diff --git a/bsp/boards/derfmega/radio.c b/bsp/boards/derfmega/radio.c index 3447eb8f08..56941f95a0 100644 --- a/bsp/boards/derfmega/radio.c +++ b/bsp/boards/derfmega/radio.c @@ -49,14 +49,14 @@ void radio_init(void) { // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; // configure the radio radio_internalWriteReg(TRX_STATE, CMD_FORCE_TRX_OFF); // turn radio off - + radio_internalWriteReg(IRQ_MASK, 0b01001100); // enable TX_END,RX_START, RX_END interrupts radio_internalWriteReg(IRQ_STATUS,0xFF); // clear all interrupts radio_internalWriteReg(ANT_DIV, RADIO_CHIP_ANTENNA); // use chip antenna @@ -64,7 +64,7 @@ void radio_init(void) { radio_internalWriteReg(TRX_CTRL_1, 0x20); // have the radio calculate CRC //busy wait until radio status is TRX_OFF while((radio_internalReadReg(TRX_STATUS) & 0x1F) != TRX_OFF); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -115,14 +115,18 @@ PORT_TIMER_WIDTH radio_getTimerPeriod(void) { void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - + // configure the radio to the right frequecy radio_internalWriteReg(PHY_CC_CCA,0x20+frequency); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { PRR1 &= ~_BV(PRTRX24); } @@ -134,11 +138,11 @@ void radio_rfOff(void) { radio_internalWriteReg(TRX_STATE, CMD_FORCE_TRX_OFF); //radio_spiWriteReg(RG_TRX_STATE, CMD_TRX_OFF); while((radio_internalReadReg(TRX_STATUS) & 0x1F) != TRX_OFF); // busy wait until done - + // wiggle debug pin //debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -148,10 +152,10 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + // load packet in TXFIFO radio_internalWriteTxFifo(packet,len); - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -159,15 +163,15 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { void radio_txEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin //debugpins_radio_set(); leds_radio_on(); - + // turn on radio's PLL radio_internalWriteReg(TRX_STATE, CMD_PLL_ON); while((radio_internalReadReg(TRX_STATUS) & 0x1F) != PLL_ON); // busy wait until done - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -175,7 +179,7 @@ void radio_txEnable(void) { void radio_txNow(void) { // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + // send packet by forcing state to TX_START radio_internalWriteReg(TRX_STATE, CMD_TX_START); // The AT86RF231 does not generate an interrupt when the radio transmits the @@ -196,16 +200,16 @@ void radio_txNow(void) { void radio_rxEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + // put radio in reception mode radio_internalWriteReg(TRX_STATE, CMD_RX_ON); // wiggle debug pin //debugpins_radio_set(); leds_radio_on(); - + // busy wait until radio really listening while((radio_internalReadReg(TRX_STATUS) & 0x1F) != RX_ON); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -221,17 +225,17 @@ void radio_getReceivedFrame(uint8_t* pBufRead, uint8_t* pLqi, bool* pCrc) { uint8_t temp_reg_value; - + //===== crc temp_reg_value = radio_internalReadReg(PHY_RSSI); *pCrc = (temp_reg_value & 0x80)>>7; // msb is whether packet passed CRC - + //===== rssi // as per section 8.4.3 of the AT86RF231, the RSSI is calculate as: // -91 + ED [dBm] temp_reg_value = radio_internalReadReg(PHY_ED_LEVEL); *pRssi = -91 + temp_reg_value; - + //===== packet radio_internalReadRxFifo(pBufRead, pLenRead, @@ -254,9 +258,9 @@ void radio_internalReadRxFifo(uint8_t* pBufRead, uint8_t* pLenRead, uint8_t maxBufLen, uint8_t* pLqi) { - + *pLenRead = TST_RX_LENGTH; - memcpy(pBufRead,&TRXFBST,*pLenRead); + memcpy(pBufRead,&TRXFBST,*pLenRead); //poipoi, see if LQI is included in the length *pLqi = *(TRXFBST + pLenRead); } @@ -273,7 +277,7 @@ uint8_t radio_isr(void) { uint8_t radio_rx_start_isr(void) { PORT_TIMER_WIDTH capturedTime; // capture the time - capturedTime = radiotimer_getCapturedTime(); + capturedTime = radiotimer_getCapturedTime(); radio_vars.state = RADIOSTATE_RECEIVING; if (radio_vars.startFrame_cb!=NULL) { // call the callback @@ -287,7 +291,7 @@ uint8_t radio_rx_start_isr(void) { uint8_t radio_trx_end_isr(void) { PORT_TIMER_WIDTH capturedTime; // capture the time - capturedTime = radiotimer_getCapturedTime(); + capturedTime = radiotimer_getCapturedTime(); radio_vars.state = RADIOSTATE_TXRX_DONE; if (radio_vars.endFrame_cb!=NULL) { // call the callback @@ -296,4 +300,4 @@ uint8_t radio_trx_end_isr(void) { return 1; } return 0; -} \ No newline at end of file +} diff --git a/bsp/boards/eldorado/radio.c b/bsp/boards/eldorado/radio.c index f5b14c8b57..51e64705e8 100644 --- a/bsp/boards/eldorado/radio.c +++ b/bsp/boards/eldorado/radio.c @@ -43,29 +43,29 @@ void radio_init(void) { MC13192_CE_PORT = 1; MC13192_ATTN_PORT = 1; MC13192_RTXEN_PORT = 1; - MC13192_CE = 1; //chip enable (SPI) + MC13192_CE = 1; //chip enable (SPI) MC13192_ATTN = 1; //modem attenuation (used to send it to sleep mode for example) MC13192_RTXEN = 0;//to enable RX or TX - + //PA: put the external PA in bypass mode: EN_PA1_DIR = 0; EN_PA2_DIR = 0; EN_PA1 = 0; EN_PA2 = 1; - - - + + + MC13192_RESET = 1;//turn radio ON while (IRQSC_IRQF == 0);//busy wait until radio is on - (void)radio_spiReadReg(0x24);// Clear MC13192 interrupts + (void)radio_spiReadReg(0x24);// Clear MC13192 interrupts IRQACK(); /* ACK the pending IRQ interrupt */ IRQPinEnable(); /* Pin Enable, IE, IRQ CLR, negative edge. */ - - - + + + //init routine: - -/* The newest version of the MC1321x now uses an updated transceiver device. + +/* The newest version of the MC1321x now uses an updated transceiver device. Although fully compliant with earlier versions of the transceiver, proper performance of the radio requires that the following modem registers be over-programmed: Register 0x31 to 0xA0C0 Register 0x34 to 0xFEC6 These registers must be over-programmed for MC1321x @@ -76,8 +76,8 @@ devices in which the modem Chip_ID Register 0x2C reads 0x6800.*/ radio_spiWriteReg(0x34,0xFEC6); } - - + + /* Please refer to document MC13192RM for hidden register initialization */ //radio_spiWriteReg(0x04,0xA08D); /* cca stuff, not needed */ radio_spiWriteReg(0x05,0x0040); /* no interrupts from this register */ @@ -92,29 +92,29 @@ devices in which the modem Chip_ID Register 0x2C reads 0x6800.*/ radio_spiWriteReg(0x1D,0x8000); /* Disable TC2. */ radio_spiWriteReg(0x1F,0x8000); /* Disable TC3. */ radio_spiWriteReg(0x21,0x8000); /* Disable TC4. */ - - + + (void)radio_spiReadReg(0x25); /* The RST_Ind Register 25 contains the reset indicator bit. Bit reset_ind is cleared during a reset and gets set if Register 25 is read after a reset and remains set */ - - - + + + /* Read the status register to clear any undesired IRQs. */ (void)radio_spiReadReg(0x24); /* Clear the status register, if set */ - + //end init routine - + //end fabien code // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - - + + //busy wait until radio status is TRX_OFF - + //radiotimer_start(0xffff);//poipoi } /* @@ -149,6 +149,10 @@ void radio_setFrequency(uint8_t frequency) { radio_spiWriteReg(LO1_NUM_ADDR,frequency-11);//in this radio they index the channels from 0 to 15 } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { //poipoi //to leave doze mode, assert ATTN then deassert it @@ -179,8 +183,8 @@ void radio_txEnable(void) { void radio_txNow(void) { // send packet by assterting the RTXEN pin MC13192_RTXEN = 1; - - + + // The AT86RF231 does not generate an interrupt when the radio transmits the // SFD, which messes up the MAC state machine. The danger is that, if we leave // this funtion like this, any radio watchdog timer will expire. @@ -224,18 +228,18 @@ void radio_getReceivedFrame(uint8_t* bufRead, len = (uint8_t) temp;//LSB of the 16-bit register value junk = SPI1S; junk = SPI1D;//as specified in the generated radio code to remove any unwanted interrupts - + if (len>2 && len<=127) { // retrieve the whole packet (including 1B SPI address, 1B length, the packet, 1B LQI) radio_spiReadRxFifo(bufRead,len); } - + //modify this function with the new variables: lenRead, maxBufLen, rssi, lqi, crc: look in telos for examples. } void radio_rfOff(void) {// turn radio off uint16_t stat_reg; - + MC13192_RTXEN = 0;//go back to idle /* The state to enter is "Acoma Doze Mode" because CLKO is made available at any setting //now put the radio in idle mode @@ -243,42 +247,42 @@ void radio_rfOff(void) {// turn radio off stat_reg = radio_spiReadReg(MODE_ADDR); stat_reg &= 0xfff8; stat_reg |= IDLE_MODE; - radio_spiWriteReg(MODE_ADDR, stat_reg); + radio_spiWriteReg(MODE_ADDR, stat_reg); //turn PA off MC13192_PA_CTRL = PA_OFF; //turn LNA off MC13192_LNA_CTRL = LNA_OFF; - + //while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != TRX_OFF); // busy wait until done */ //revisit this function REVIEW poipoi - + } //=========================== private ========================================= void radio_spiWriteReg(uint8_t reg_addr, uint16_t reg_setting) { uint8_t u8TempValue=0; - + //clear interrupts u8TempValue = SPI1S; u8TempValue = SPI1D; - MC13192_IRQ_Disable(); // Necessary to prevent double SPI access + MC13192_IRQ_Disable(); // Necessary to prevent double SPI access MC13192_CE = 0; // Enables MC13192 SPI - - SPI1D = reg_addr & 0x3F; // Write the command + + SPI1D = reg_addr & 0x3F; // Write the command while (!(SPI1S_SPRF)); // busywait - u8TempValue = SPI1D; //Clear receive data register. SPI entirely + u8TempValue = SPI1D; //Clear receive data register. SPI entirely + + SPI1D = ((uint8_t)(reg_setting >> 8)); /* Write MSB */ + while (!(SPI1S_SPRF)); + u8TempValue = SPI1D; + - SPI1D = ((uint8_t)(reg_setting >> 8)); /* Write MSB */ - while (!(SPI1S_SPRF)); - u8TempValue = SPI1D; - - SPI1D = ((uint8_t)(reg_setting & 0x00FF)); /* Write LSB */ - while (!(SPI1S_SPRF)); + while (!(SPI1S_SPRF)); u8TempValue = SPI1D; - - + + u8TempValue = SPI1S;//to remove interrupts? MC13192_CE = 1; //MC13192_IRQ_Enable(); @@ -291,25 +295,25 @@ uint16_t radio_spiReadReg(uint8_t reg_addr) { //clear interrupts u8TempValue = SPI1S; u8TempValue = SPI1D; - + MC13192_IRQ_Disable(); /* Necessary to prevent double SPI access */ MC13192_CE = 0; /* Enables MC13192 SPI */ - - - SPI1D = ((reg_addr & 0x3f) | 0x80); // Write the command + + + SPI1D = ((reg_addr & 0x3f) | 0x80); // Write the command while (!(SPI1S_SPRF)); // busywait - u8TempValue = SPI1D; //Clear receive data register. SPI entirely - - + u8TempValue = SPI1D; //Clear receive data register. SPI entirely + + SPI1D = reg_addr| 0x80;; // Dummy write. while (!(SPI1S_SPRF)); // busywait ((uint8_t*)u16Data)[0] = SPI1D; /* MSB */ - - + + SPI1D = reg_addr| 0x80;; // Dummy write. while (!(SPI1S_SPRF)); // busywait ((uint8_t*)u16Data)[1] = SPI1D; /* LSB */ - + u8TempValue = SPI1S; MC13192_CE = 1; /* Disables MC13192 SPI */ //MC13192_IRQ_Enable(); /* Restore MC13192 interrupt status */ @@ -317,30 +321,30 @@ uint16_t radio_spiReadReg(uint8_t reg_addr) { } void radio_spiWriteTxFifo(uint8_t* bufToWrite, uint8_t lenToWrite) { - uint8_t bufToWrite2[127+1]; + uint8_t bufToWrite2[127+1]; uint8_t spi_rx_buffer[127+1]; uint8_t temp;//used as garbage value uint16_t lenvalue;//used because we need to write 16 bit values inside registers - + //the compiler is being stupid lenvalue = (uint16_t)(lenToWrite << 8);//lsB of lenvalue is the length (this length already includes the two crc bytes) lenvalue = lenvalue>>8; - + radio_spiWriteReg(0x03, lenvalue);/*Choose RAM register 1; Update the TX packet length field (2 extra bytes for CRC) */ temp = SPI1S; temp = SPI1D;//as specified in the generated radio code (removes interrupts and such) - + lenToWrite -= 2;//we need not write two empty bytes to the radio like for other manufacturers. //now check if we have an even number of bytes and update accordingly (the radio requires an even number) if(lenToWrite&0x01) lenToWrite +=1; - + //[lenToWrite+1];//recreate packet with TX_PKT spi register address (0x02) bufToWrite2[0] = TX_PKT; for(counter=0;counter CC2538_RF_CHANNEL_MAX)) { while(1); } - + /* Changes to FREQCTRL take effect after the next recalibration */ HWREG(RFCORE_XREG_FREQCTRL) = (CC2538_RF_CHANNEL_MIN + (frequency - CC2538_RF_CHANNEL_MIN) * CC2538_RF_CHANNEL_SPACING); - + //radio_on(); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +// configure the radio to output at least @power dBm, or the maximum available power +void radio_setTxPower(int8_t power) { + uint8_t i; + uint8_t reg_val; + + // loop to find at-least :power: dBm in the look-up table + // if the max output power of a chip is higher than :power:, we configure the maximum + reg_val = cc2538_output_power[0].register_val; + for(i = sizeof(cc2538_output_power) / sizeof(radio_output_power_config_t) - 1; i >= 0; --i) { + if(power <= cc2538_output_power[i].power_dbm) { + reg_val = cc2538_output_power[i].register_val; + break; + } + } + + HWREG(RFCORE_XREG_TXPOWER) = reg_val; +} + void radio_rfOn(void) { //radio_on(); } void radio_rfOff(void) { - + // change state radio_vars.state = RADIOSTATE_TURNING_OFF; radio_off(); @@ -216,7 +251,7 @@ void radio_rfOff(void) { leds_radio_off(); //enable radio interrupts disable_radio_interrupts(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -225,49 +260,49 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { uint8_t i=0; - + // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + // load packet in TXFIFO /* When we transmit in very quick bursts, make sure previous transmission is not still in progress before re-writing to the TX FIFO */ while(HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_TX_ACTIVE); - + CC2538_RF_CSP_ISFLUSHTX(); - + /* Send the phy length byte first */ HWREG(RFCORE_SFR_RFDATA) = len; //crc len is included - + for(i = 0; i < len; i++) { HWREG(RFCORE_SFR_RFDATA) = packet[i]; } - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } void radio_txEnable(void) { - + // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + //do nothing -- radio is activated by the strobe on rx or tx //radio_rfOn(); - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } void radio_txNow(void) { PORT_TIMER_WIDTH count; - + // change state radio_vars.state = RADIOSTATE_TRANSMITTING; @@ -290,17 +325,17 @@ void radio_txNow(void) { //===== RX void radio_rxEnable(void) { - + // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + //enable radio interrupts - + // do nothing as we do not want to receive anything yet. // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -308,11 +343,11 @@ void radio_rxEnable(void) { void radio_rxNow(void) { //empty buffer before receiving //CC2538_RF_CSP_ISFLUSHRX(); - + //enable radio interrupts CC2538_RF_CSP_ISFLUSHRX(); enable_radio_interrupts(); - + CC2538_RF_CSP_ISRXON(); // busy wait until radio really listening while(!((HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_RX_ACTIVE))); @@ -325,49 +360,49 @@ void radio_getReceivedFrame(uint8_t* pBufRead, uint8_t* pLqi, bool* pCrc) { uint8_t crc_corr,i; - + uint8_t len=0; - + /* Check the length */ len = HWREG(RFCORE_SFR_RFDATA); //first byte is len - - + + /* Check for validity */ if(len > CC2538_RF_MAX_PACKET_LEN) { /* wrong len */ CC2538_RF_CSP_ISFLUSHRX(); return; } - - + + if(len <= CC2538_RF_MIN_PACKET_LEN) { //too short CC2538_RF_CSP_ISFLUSHRX(); return; } - + //check if this fits to the buffer if(len > maxBufLen) { CC2538_RF_CSP_ISFLUSHRX(); return; } - + // when reading the packet from the RX buffer, you get the following: // - *[1B] length byte // - [0-125B] packet (excluding CRC) // - [1B] RSSI // - *[2B] CRC - + //skip first byte is len for(i = 0; i < len - 2; i++) { pBufRead[i] = HWREG(RFCORE_SFR_RFDATA); } - + *pRssi = ((int8_t)(HWREG(RFCORE_SFR_RFDATA)) - RSSI_OFFSET); crc_corr = HWREG(RFCORE_SFR_RFDATA); *pCrc = crc_corr & CRC_BIT_MASK; *pLenRead = len; - + //flush it CC2538_RF_CSP_ISFLUSHRX(); } @@ -398,7 +433,7 @@ void radio_off(void){ /* Wait for ongoing TX to complete (e.g. this could be an outgoing ACK) */ while(HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_TX_ACTIVE); //CC2538_RF_CSP_ISFLUSHRX(); - + /* Don't turn off if we are off as this will trigger a Strobe Error */ if(HWREG(RFCORE_XREG_RXENABLE) != 0) { CC2538_RF_CSP_ISRFOFF(); @@ -436,22 +471,22 @@ kick_scheduler_t radio_isr(void) { void radio_isr_internal(void) { volatile PORT_TIMER_WIDTH capturedTime; uint8_t irq_status0,irq_status1; - + debugpins_isr_set(); - + // capture the time capturedTime = sctimer_readCounter(); - + // reading IRQ_STATUS irq_status0 = HWREG(RFCORE_SFR_RFIRQF0); irq_status1 = HWREG(RFCORE_SFR_RFIRQF1); - + IntPendClear(INT_RFCORERTX); - + //clear interrupt HWREG(RFCORE_SFR_RFIRQF0) = 0; HWREG(RFCORE_SFR_RFIRQF1) = 0; - + //STATUS0 Register // start of frame event if ((irq_status0 & RFCORE_SFR_RFIRQF0_SFD) == RFCORE_SFR_RFIRQF0_SFD) { @@ -467,7 +502,7 @@ void radio_isr_internal(void) { while(1); } } - + //or RXDONE is full -- we have a packet. if (((irq_status0 & RFCORE_SFR_RFIRQF0_RXPKTDONE) == RFCORE_SFR_RFIRQF0_RXPKTDONE)) { // change state @@ -482,7 +517,7 @@ void radio_isr_internal(void) { while(1); } } - + // or FIFOP is full -- we have a packet. if (((irq_status0 & RFCORE_SFR_RFIRQF0_FIFOP) == RFCORE_SFR_RFIRQF0_FIFOP)) { // change state @@ -497,7 +532,7 @@ void radio_isr_internal(void) { while(1); } } - + //STATUS1 Register // end of frame event --either end of tx . if (((irq_status1 & RFCORE_SFR_RFIRQF1_TXDONE) == RFCORE_SFR_RFIRQF1_TXDONE)) { @@ -514,15 +549,15 @@ void radio_isr_internal(void) { } } debugpins_isr_clr(); - + return; } void radio_error_isr(void){ uint8_t rferrm; - + rferrm = (uint8_t)HWREG(RFCORE_XREG_RFERRM); - + if ((HWREG(RFCORE_XREG_RFERRM) & (((0x02)< CC2538_RF_CHANNEL_MAX)) { while(1); } - + /* Changes to FREQCTRL take effect after the next recalibration */ HWREG(RFCORE_XREG_FREQCTRL) = (CC2538_RF_CHANNEL_MIN + (frequency - CC2538_RF_CHANNEL_MIN) * CC2538_RF_CHANNEL_SPACING); - + //radio_on(); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { //radio_on(); } void radio_rfOff(void) { - + // change state radio_vars.state = RADIOSTATE_TURNING_OFF; radio_off(); @@ -216,7 +220,7 @@ void radio_rfOff(void) { leds_radio_off(); //enable radio interrupts disable_radio_interrupts(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -225,49 +229,49 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { uint8_t i=0; - + // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + // load packet in TXFIFO /* When we transmit in very quick bursts, make sure previous transmission is not still in progress before re-writing to the TX FIFO */ while(HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_TX_ACTIVE); - + CC2538_RF_CSP_ISFLUSHTX(); - + /* Send the phy length byte first */ HWREG(RFCORE_SFR_RFDATA) = len; //crc len is included - + for(i = 0; i < len; i++) { HWREG(RFCORE_SFR_RFDATA) = packet[i]; } - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } void radio_txEnable(void) { - + // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + //do nothing -- radio is activated by the strobe on rx or tx //radio_rfOn(); - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } void radio_txNow(void) { PORT_TIMER_WIDTH count; - + // change state radio_vars.state = RADIOSTATE_TRANSMITTING; @@ -290,17 +294,17 @@ void radio_txNow(void) { //===== RX void radio_rxEnable(void) { - + // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + //enable radio interrupts - + // do nothing as we do not want to receive anything yet. // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -308,11 +312,11 @@ void radio_rxEnable(void) { void radio_rxNow(void) { //empty buffer before receiving //CC2538_RF_CSP_ISFLUSHRX(); - + //enable radio interrupts CC2538_RF_CSP_ISFLUSHRX(); enable_radio_interrupts(); - + CC2538_RF_CSP_ISRXON(); // busy wait until radio really listening while(!((HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_RX_ACTIVE))); @@ -325,49 +329,49 @@ void radio_getReceivedFrame(uint8_t* pBufRead, uint8_t* pLqi, bool* pCrc) { uint8_t crc_corr,i; - + uint8_t len=0; - + /* Check the length */ len = HWREG(RFCORE_SFR_RFDATA); //first byte is len - - + + /* Check for validity */ if(len > CC2538_RF_MAX_PACKET_LEN) { /* wrong len */ CC2538_RF_CSP_ISFLUSHRX(); return; } - - + + if(len <= CC2538_RF_MIN_PACKET_LEN) { //too short CC2538_RF_CSP_ISFLUSHRX(); return; } - + //check if this fits to the buffer if(len > maxBufLen) { CC2538_RF_CSP_ISFLUSHRX(); return; } - + // when reading the packet from the RX buffer, you get the following: // - *[1B] length byte // - [0-125B] packet (excluding CRC) // - [1B] RSSI // - *[2B] CRC - + //skip first byte is len for(i = 0; i < len - 2; i++) { pBufRead[i] = HWREG(RFCORE_SFR_RFDATA); } - + *pRssi = ((int8_t)(HWREG(RFCORE_SFR_RFDATA)) - RSSI_OFFSET); crc_corr = HWREG(RFCORE_SFR_RFDATA); *pCrc = crc_corr & CRC_BIT_MASK; *pLenRead = len; - + //flush it CC2538_RF_CSP_ISFLUSHRX(); } @@ -398,7 +402,7 @@ void radio_off(void){ /* Wait for ongoing TX to complete (e.g. this could be an outgoing ACK) */ while(HWREG(RFCORE_XREG_FSMSTAT1) & RFCORE_XREG_FSMSTAT1_TX_ACTIVE); //CC2538_RF_CSP_ISFLUSHRX(); - + /* Don't turn off if we are off as this will trigger a Strobe Error */ if(HWREG(RFCORE_XREG_RXENABLE) != 0) { CC2538_RF_CSP_ISRFOFF(); @@ -436,22 +440,22 @@ kick_scheduler_t radio_isr(void) { void radio_isr_internal(void) { volatile PORT_TIMER_WIDTH capturedTime; uint8_t irq_status0,irq_status1; - + debugpins_isr_set(); - + // capture the time capturedTime = sctimer_readCounter(); - + // reading IRQ_STATUS irq_status0 = HWREG(RFCORE_SFR_RFIRQF0); irq_status1 = HWREG(RFCORE_SFR_RFIRQF1); - + IntPendClear(INT_RFCORERTX); - + //clear interrupt HWREG(RFCORE_SFR_RFIRQF0) = 0; HWREG(RFCORE_SFR_RFIRQF1) = 0; - + //STATUS0 Register // start of frame event if ((irq_status0 & RFCORE_SFR_RFIRQF0_SFD) == RFCORE_SFR_RFIRQF0_SFD) { @@ -467,7 +471,7 @@ void radio_isr_internal(void) { while(1); } } - + //or RXDONE is full -- we have a packet. if (((irq_status0 & RFCORE_SFR_RFIRQF0_RXPKTDONE) == RFCORE_SFR_RFIRQF0_RXPKTDONE)) { // change state @@ -482,7 +486,7 @@ void radio_isr_internal(void) { while(1); } } - + // or FIFOP is full -- we have a packet. if (((irq_status0 & RFCORE_SFR_RFIRQF0_FIFOP) == RFCORE_SFR_RFIRQF0_FIFOP)) { // change state @@ -497,7 +501,7 @@ void radio_isr_internal(void) { while(1); } } - + //STATUS1 Register // end of frame event --either end of tx . if (((irq_status1 & RFCORE_SFR_RFIRQF1_TXDONE) == RFCORE_SFR_RFIRQF1_TXDONE)) { @@ -514,15 +518,15 @@ void radio_isr_internal(void) { } } debugpins_isr_clr(); - + return; } void radio_error_isr(void){ uint8_t rferrm; - + rferrm = (uint8_t)HWREG(RFCORE_XREG_RFERRM); - + if ((HWREG(RFCORE_XREG_RFERRM) & (((0x02)<= 0; --i) { + if(power <= cc2538_output_power[i].power_dbm) { + reg_val = cc2538_output_power[i].register_val; + break; + } + } + + HWREG(RFCORE_XREG_TXPOWER) = reg_val; +} + void radio_rfOn(void) { //radio_on(); } diff --git a/bsp/boards/python/openwsnmodule_obj.h b/bsp/boards/python/openwsnmodule_obj.h index aa3ad3267e..bf6a36cef6 100644 --- a/bsp/boards/python/openwsnmodule_obj.h +++ b/bsp/boards/python/openwsnmodule_obj.h @@ -34,6 +34,7 @@ #include "c6t_obj.h" #include "cexample_obj.h" #include "cjoin_obj.h" +#include "cbenchmark_obj.h" #include "cinfo_obj.h" #include "cleds_obj.h" #include "cstorm_obj.h" @@ -235,6 +236,7 @@ struct OpenMote { rrt_vars_t rrt_vars; //tohlone_vars_t tohlone_vars; cjoin_vars_t cjoin_vars; + cbenchmark_vars_t cbenchmark_vars; uecho_vars_t uecho_vars; uinject_vars_t uinject_vars; userialbridge_vars_t userialbridge_vars; diff --git a/bsp/boards/python/radio_obj.c b/bsp/boards/python/radio_obj.c index 093b989019..1c7a48ab1e 100644 --- a/bsp/boards/python/radio_obj.c +++ b/bsp/boards/python/radio_obj.c @@ -14,26 +14,26 @@ void radio_setStartFrameCb(OpenMote* self, radio_capture_cbt cb) { - + #ifdef TRACE_ON printf("C@0x%x: radio_setStartFrameCb(cb=0x%x)... \n",self,cb); #endif - + self->radio_icb.startFrame_cb = cb; - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif } void radio_setEndFrameCb(OpenMote* self, radio_capture_cbt cb) { - + #ifdef TRACE_ON printf("C@0x%x: radio_setEndFrameCb(cb=0x%x)... \n",self,cb); #endif - + self->radio_icb.endFrame_cb = cb; - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -45,11 +45,11 @@ void radio_setEndFrameCb(OpenMote* self, radio_capture_cbt cb) { void radio_init(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_init()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_init],NULL); if (result == NULL) { @@ -57,7 +57,7 @@ void radio_init(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -67,11 +67,11 @@ void radio_init(OpenMote* self) { void radio_reset(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_reset()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_reset],NULL); if (result == NULL) { @@ -79,7 +79,7 @@ void radio_reset(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -90,11 +90,11 @@ void radio_reset(OpenMote* self) { void radio_setFrequency(OpenMote* self, uint8_t frequency) { PyObject* result; PyObject* arglist; - + #ifdef TRACE_ON printf("C@0x%x: radio_setFrequency(frequency=%d)... \n",self,frequency); #endif - + // forward to Python arglist = Py_BuildValue("(i)",frequency); result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_setFrequency],arglist); @@ -104,19 +104,23 @@ void radio_setFrequency(OpenMote* self, uint8_t frequency) { } Py_DECREF(result); Py_DECREF(arglist); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif } +void radio_setTxPower(OpenMote* self, int8_t power) { + // TODO +} + void radio_rfOn(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_rfOn()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_rfOn],NULL); if (result == NULL) { @@ -124,7 +128,7 @@ void radio_rfOn(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -132,11 +136,11 @@ void radio_rfOn(OpenMote* self) { void radio_rfOff(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_rfOff()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_rfOff],NULL); if (result == NULL) { @@ -144,7 +148,7 @@ void radio_rfOff(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -159,11 +163,11 @@ void radio_loadPacket(OpenMote* self, uint8_t* packet, uint16_t len) { PyObject* item; int8_t i; int res; - + #ifdef TRACE_ON printf("C@0x%x: radio_loadPacket(len=%d)... \n",self,len); #endif - + // forward to Python pkt = PyList_New(len); for (i=0;icallback[MOTE_NOTIF_radio_txEnable],NULL); if (result == NULL) { @@ -199,7 +203,7 @@ void radio_txEnable(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -207,11 +211,11 @@ void radio_txEnable(OpenMote* self) { void radio_txNow(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_txNow()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_txNow],NULL); if (result == NULL) { @@ -219,7 +223,7 @@ void radio_txNow(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -229,11 +233,11 @@ void radio_txNow(OpenMote* self) { void radio_rxEnable(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_rxEnable()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_rxEnable],NULL); if (result == NULL) { @@ -241,7 +245,7 @@ void radio_rxEnable(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -249,11 +253,11 @@ void radio_rxEnable(OpenMote* self) { void radio_rxNow(OpenMote* self) { PyObject* result; - + #ifdef TRACE_ON printf("C@0x%x: radio_rxNow()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_rxNow],NULL); if (result == NULL) { @@ -261,7 +265,7 @@ void radio_rxNow(OpenMote* self) { return; } Py_DECREF(result); - + #ifdef TRACE_ON printf("C@0x%x: ...done.\n",self); #endif @@ -279,18 +283,18 @@ void radio_getReceivedFrame(OpenMote* self, PyObject* subitem; int8_t lenRead; int8_t i; - + #ifdef TRACE_ON printf("C@0x%x: radio_getReceivedFrame()... \n",self); #endif - + // forward to Python result = PyObject_CallObject(self->callback[MOTE_NOTIF_radio_getReceivedFrame],NULL); if (result == NULL) { printf("[CRITICAL] radio_getReceivedFrame() returned NULL\r\n"); return; } - + // verify if (!PySequence_Check(result)) { printf("[CRITICAL] radio_getReceivedFrame() did not return a tuple\r\n"); @@ -300,9 +304,9 @@ void radio_getReceivedFrame(OpenMote* self, printf("[CRITICAL] radio_getReceivedFrame() did not return a tuple of exactly 4 elements %d\r\n",PyList_Size(result)); return; } - + //==== item 0: rxBuffer - + item = PyTuple_GetItem(result,0); lenRead = PyList_Size(item); *pLenRead = lenRead; @@ -311,19 +315,19 @@ void radio_getReceivedFrame(OpenMote* self, subitem = PyList_GetItem(item, i); pBufRead[i] = (uint8_t)PyInt_AsLong(subitem); } - + //==== item 1: rssi - + item = PyTuple_GetItem(result,1); *pRssi = (int8_t)PyInt_AsLong(item); - + //==== item 2: lqi - + item = PyTuple_GetItem(result,2); *pLqi = (uint8_t)PyInt_AsLong(item); - + //==== item 3: crc - + item = PyTuple_GetItem(result,3); *pCrc = (uint8_t)PyInt_AsLong(item); } diff --git a/bsp/boards/radio.h b/bsp/boards/radio.h index 93cd0d9924..46f9fa7f6a 100644 --- a/bsp/boards/radio.h +++ b/bsp/boards/radio.h @@ -22,7 +22,7 @@ \brief Current state of the radio. \note This radio driver is very minimal in that it does not follow a state machine. - It is up to the MAC layer to ensure that the different radio operations + It is up to the MAC layer to ensure that the different radio operations are called in the righr order. The radio keeps a state for debugging purposes only. */ typedef enum { @@ -42,6 +42,17 @@ typedef enum { RADIOSTATE_TURNING_OFF = 0x0d, ///< Turning the RF chain off. } radio_state_t; +/** +\brief Helper structure for power configuration to be defined by platforms with specific values. + +\note Not all values are supported by all platforms. In case the value is not supported, the closest higher +value SHOULD be configured by the implementation in radio_setTxPower() function. +*/ +typedef struct { + int8_t power_dbm; + uint8_t register_val; +} radio_output_power_config_t; + //=========================== typedef ========================================= typedef void (*radio_capture_cbt)(PORT_TIMER_WIDTH timestamp); @@ -58,6 +69,7 @@ void radio_setEndFrameCb(radio_capture_cbt cb); void radio_reset(void); // RF admin void radio_setFrequency(uint8_t frequency); +void radio_setTxPower(int8_t power); // Transmit power configuration in dBm void radio_rfOn(void); void radio_rfOff(void); // TX diff --git a/bsp/boards/scum/radio.c b/bsp/boards/scum/radio.c index b590fc5c7c..345dde2800 100644 --- a/bsp/boards/scum/radio.c +++ b/bsp/boards/scum/radio.c @@ -28,7 +28,7 @@ typedef struct { radio_capture_cbt endFrame_cb; uint8_t radio_tx_buffer[MAXLENGTH_TRX_BUFFER] __attribute__ ((aligned (4))); uint8_t radio_rx_buffer[MAXLENGTH_TRX_BUFFER] __attribute__ ((aligned (4))); - radio_state_t state; + radio_state_t state; } radio_vars_t; radio_vars_t radio_vars; @@ -43,7 +43,7 @@ void radio_init(void) { // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; #ifdef SLOT_FSM_IMPLEMENTATION_MULTIPLE_TIMER_INTERRUPT @@ -71,7 +71,7 @@ void radio_init(void) { RX_OVERFLOW_ERROR_EN | \ RX_CRC_ERROR_EN | \ RX_CUTOFF_ERROR_EN; - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -96,14 +96,18 @@ void radio_reset(void) { void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - + // not support by SCuM yet - - + + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { // clear reset pin RFCONTROLLER_REG__CONTROL &= ~RX_RESET; @@ -115,7 +119,7 @@ void radio_rfOff(void) { // turn SCuM radio off RFCONTROLLER_REG__CONTROL = RX_STOP; - + // wiggle debug pin debugpins_radio_clr(); leds_radio_off(); @@ -128,14 +132,14 @@ void radio_rfOff(void) { #ifdef SLOT_FSM_IMPLEMENTATION_MULTIPLE_TIMER_INTERRUPT void radio_loadPacket_prepare(uint8_t* packet, uint16_t len){ - + radio_vars.state = RADIOSTATE_LOADING_PACKET; - + memcpy(&radio_vars.radio_tx_buffer[0],packet,len); - + RFCONTROLLER_REG__TX_DATA_ADDR = &(radio_vars.radio_tx_buffer[0]); RFCONTROLLER_REG__TX_PACK_LEN = len; - + // will be loaded when load timer fired, change the state in advance radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -145,7 +149,7 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { uint8_t i; // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + memcpy(&radio_vars.radio_tx_buffer[0],packet,len); // load packet in TXFIFO @@ -156,7 +160,7 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { // add some delay for loading for (i=0;i<0xff;i++); - + radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -166,7 +170,7 @@ void radio_txEnable(void) { radio_vars.state = RADIOSTATE_ENABLING_TX; // not support by SCuM - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); @@ -188,7 +192,7 @@ void radio_rxPacket_prepare(void){ } void radio_rxEnable(void) { - + // change state radio_vars.state = RADIOSTATE_ENABLING_RX; DMA_REG__RF_RX_ADDR = &(radio_vars.radio_rx_buffer[0]); @@ -197,7 +201,7 @@ void radio_rxEnable(void) { // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_LISTENING; @@ -206,11 +210,11 @@ void radio_rxEnable(void) { void radio_rxEnable_scum(void){ // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -225,17 +229,17 @@ void radio_getReceivedFrame(uint8_t* pBufRead, int8_t* pRssi, uint8_t* pLqi, bool* pCrc) { - + //===== crc *pCrc = DEFAULT_CRC_CHECK; - + //===== rssi *pRssi = DEFAULT_RSSI; - + //===== length *pLenRead = radio_vars.radio_rx_buffer[0]; - - //===== packet + + //===== packet memcpy(pBufRead,&(radio_vars.radio_rx_buffer[1]),*pLenRead); } @@ -246,20 +250,20 @@ void radio_getReceivedFrame(uint8_t* pBufRead, //=========================== interrupt handlers ============================== kick_scheduler_t radio_isr(void) { - + PORT_TIMER_WIDTH capturedTime; - + PORT_TIMER_WIDTH irq_status = RFCONTROLLER_REG__INT; PORT_TIMER_WIDTH irq_error = RFCONTROLLER_REG__ERROR; - + debugpins_isr_set(); - + #ifdef SLOT_FSM_IMPLEMENTATION_MULTIPLE_TIMER_INTERRUPT #else capturedTime = sctimer_readCounter(); #endif if (irq_status & TX_SFD_DONE_INT || irq_status & RX_SFD_DONE_INT){ - // SFD is just sent or received, check the specific interruption and + // SFD is just sent or received, check the specific interruption and // change the radio state accordingly if (irq_status & TX_SFD_DONE_INT) { #ifdef SLOT_FSM_IMPLEMENTATION_MULTIPLE_TIMER_INTERRUPT @@ -287,7 +291,7 @@ kick_scheduler_t radio_isr(void) { return KICK_SCHEDULER; } } - + if (irq_status & TX_SEND_DONE_INT || irq_status & RX_DONE_INT){ if (irq_status & TX_SEND_DONE_INT) { #ifdef SLOT_FSM_IMPLEMENTATION_MULTIPLE_TIMER_INTERRUPT @@ -316,13 +320,13 @@ kick_scheduler_t radio_isr(void) { while(1); } } - + if (irq_status & TX_LOAD_DONE_INT){ RFCONTROLLER_REG__INT_CLEAR = TX_LOAD_DONE_INT; } - + if (irq_error == 0) { - // error happens during the operation of radio. Print out the error here. + // error happens during the operation of radio. Print out the error here. // To Be Done. add error description deifinition for this type of errors. RFCONTROLLER_REG__ERROR_CLEAR = irq_error; } diff --git a/bsp/boards/silabs-ezr32wg/radio.c b/bsp/boards/silabs-ezr32wg/radio.c index 348a2d93c0..9e1c6c9eb7 100644 --- a/bsp/boards/silabs-ezr32wg/radio.c +++ b/bsp/boards/silabs-ezr32wg/radio.c @@ -117,6 +117,10 @@ void radio_setFrequency(uint8_t frequency) { radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { } diff --git a/bsp/chips/at86rf215/radio.c b/bsp/chips/at86rf215/radio.c index ea830477af..9f19625d48 100644 --- a/bsp/chips/at86rf215/radio.c +++ b/bsp/chips/at86rf215/radio.c @@ -206,6 +206,10 @@ void radio_setFrequency(uint16_t channel) { radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { //put the radio in the TRXPREP state at86rf215_spiStrobe(CMD_RF_TRXOFF, ATMEL_FREQUENCY_TYPE); diff --git a/bsp/chips/at86rf231/radio.c b/bsp/chips/at86rf231/radio.c index 8a604d3787..3aed72db74 100644 --- a/bsp/chips/at86rf231/radio.c +++ b/bsp/chips/at86rf231/radio.c @@ -20,11 +20,28 @@ typedef struct { radio_capture_cbt startFrame_cb; radio_capture_cbt endFrame_cb; - radio_state_t state; + radio_state_t state; } radio_vars_t; radio_vars_t radio_vars; +/* TX Power dBm lookup table. Rounded values from AT86RF231 datasheet */ +static const radio_output_power_config_t at86rf231_output_power[] = { + { 3, 0x00 }, + { 2, 0x02 }, // 2.3 dBm + { 1, 0x04 }, // 1.3 dBm + { 0, 0x06 }, + { -1, 0x07 }, + { -2, 0x08 }, + { -3, 0x09 }, + { -4, 0x0a }, + { -5, 0x0b }, + { -7, 0x0c }, + { -9, 0x0d }, + { -12, 0x0e }, + { -17, 0x0f }, +}; + //=========================== prototypes ====================================== void radio_spiWriteReg(uint8_t reg_addr, uint8_t reg_setting); @@ -44,13 +61,13 @@ void radio_init(void) { // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; - + // configure the radio radio_spiWriteReg(RG_TRX_STATE, CMD_FORCE_TRX_OFF); // turn radio off - + radio_spiWriteReg(RG_IRQ_MASK, (AT_IRQ_RX_START| AT_IRQ_TRX_END)); // tell radio to fire interrupt on TRX_END and RX_START radio_spiReadReg(RG_IRQ_STATUS); // deassert the interrupt pin in case is high @@ -63,9 +80,9 @@ void radio_init(void) { radio_spiWriteReg(RG_PHY_TX_PWR, (0x3<<6)|INIT_TX_POWER); //busy wait until radio status is TRX_OFF - + while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != TRX_OFF); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -89,14 +106,33 @@ void radio_reset(void) { void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - + // configure the radio to the right frequecy radio_spiWriteReg(RG_PHY_CC_CCA,0x20+frequency); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +// configure the radio to output at least @power dBm, or the maximum available power +void radio_setTxPower(int8_t power) { + uint8_t i; + uint8_t reg_val; + + // loop to find at-least :power: dBm in the look-up table + // if the max output power of a chip is higher than :power:, we configure the maximum + reg_val = at86rf231_output_power[0].register_val; + for(i = sizeof(at86rf231_output_power) / sizeof(radio_output_power_config_t) - 1; i >= 0; --i) { + if(power <= at86rf231_output_power[i].power_dbm) { + reg_val = at86rf231_output_power[i].register_val; + break; + } + } + + // configure the radio + radio_spiWriteReg(RG_PHY_TX_PWR, (0x3<<6) | reg_val); +} + void radio_rfOn(void) { PORT_PIN_RADIO_RESET_LOW(); PORT_PIN_RADIO_RESET_HIGH(); @@ -110,11 +146,11 @@ void radio_rfOff(void) { radio_spiWriteReg(RG_TRX_STATE, CMD_FORCE_TRX_OFF); //radio_spiWriteReg(RG_TRX_STATE, CMD_TRX_OFF); while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != TRX_OFF); // busy wait until done - + // wiggle debug pin debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -124,10 +160,10 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + // load packet in TXFIFO radio_spiWriteTxFifo(packet,len); - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -135,15 +171,15 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { void radio_txEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // turn on radio's PLL radio_spiWriteReg(RG_TRX_STATE, CMD_PLL_ON); while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != PLL_ON); // busy wait until done - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -152,11 +188,11 @@ void radio_txNow(void) { PORT_TIMER_WIDTH val; // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + // send packet by pulsing the SLP_TR_CNTL pin PORT_PIN_RADIO_SLP_TR_CNTL_HIGH(); PORT_PIN_RADIO_SLP_TR_CNTL_LOW(); - + // The AT86RF231 does not generate an interrupt when the radio transmits the // SFD, which messes up the MAC state machine. The danger is that, if we leave // this funtion like this, any radio watchdog timer will expire. @@ -176,17 +212,17 @@ void radio_txNow(void) { void radio_rxEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + // put radio in reception mode radio_spiWriteReg(RG_TRX_STATE, CMD_RX_ON); - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // busy wait until radio really listening while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != RX_ON); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -202,17 +238,17 @@ void radio_getReceivedFrame(uint8_t* pBufRead, uint8_t* pLqi, bool* pCrc) { uint8_t temp_reg_value; - + //===== crc temp_reg_value = radio_spiReadReg(RG_PHY_RSSI); *pCrc = (temp_reg_value & 0x80)>>7; // msb is whether packet passed CRC - + //===== rssi // as per section 8.4.3 of the AT86RF231, the RSSI is calculate as: // -91 + ED [dBm] temp_reg_value = radio_spiReadReg(RG_PHY_ED_LEVEL); *pRssi = -91 + temp_reg_value; - + //===== packet radio_spiReadRxFifo(pBufRead, pLenRead, @@ -249,10 +285,10 @@ uint8_t radio_spiReadRadioInfo(void) { void radio_spiWriteReg(uint8_t reg_addr, uint8_t reg_setting) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[2]; - + spi_tx_buffer[0] = (0xC0 | reg_addr); // turn addess in a 'reg write' address spi_tx_buffer[1] = reg_setting; - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -265,10 +301,10 @@ void radio_spiWriteReg(uint8_t reg_addr, uint8_t reg_setting) { uint8_t radio_spiReadReg(uint8_t reg_addr) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[2]; - + spi_tx_buffer[0] = (0x80 | reg_addr); // turn addess in a 'reg read' address spi_tx_buffer[1] = 0x00; // send a no_operation command just to get the reg value - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -276,7 +312,7 @@ uint8_t radio_spiReadReg(uint8_t reg_addr) { sizeof(spi_rx_buffer), SPI_FIRST, SPI_LAST); - + return spi_rx_buffer[1]; } @@ -286,10 +322,10 @@ uint8_t radio_spiReadReg(uint8_t reg_addr) { void radio_spiWriteTxFifo(uint8_t* bufToWrite, uint8_t lenToWrite) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[1+1+127]; // 1B SPI address, 1B length, max. 127B data - + spi_tx_buffer[0] = 0x60; // SPI destination address for TXFIFO spi_tx_buffer[1] = lenToWrite; // length byte - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -297,7 +333,7 @@ void radio_spiWriteTxFifo(uint8_t* bufToWrite, uint8_t lenToWrite) { sizeof(spi_rx_buffer), SPI_FIRST, SPI_NOTLAST); - + spi_txrx(bufToWrite, lenToWrite, SPI_BUFFER, @@ -321,9 +357,9 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, // - *[1B] LQI uint8_t spi_tx_buffer[125]; uint8_t spi_rx_buffer[3]; - + spi_tx_buffer[0] = 0x20; - + // 2 first bytes spi_txrx(spi_tx_buffer, 2, @@ -332,12 +368,12 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, sizeof(spi_rx_buffer), SPI_FIRST, SPI_NOTLAST); - + *pLenRead = spi_rx_buffer[1]; - + if (*pLenRead>2 && *pLenRead<=127) { // valid length - + //read packet spi_txrx(spi_tx_buffer, *pLenRead, @@ -346,7 +382,7 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, 125, SPI_NOTFIRST, SPI_NOTLAST); - + // CRC (2B) and LQI (1B) spi_txrx(spi_tx_buffer, 2+1, @@ -355,12 +391,12 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, 3, SPI_NOTFIRST, SPI_LAST); - + *pLqi = spi_rx_buffer[2]; - + } else { // invalid length - + // read a just byte to close spi spi_txrx(spi_tx_buffer, 1, @@ -385,7 +421,7 @@ kick_scheduler_t radio_isr(void) { // reading IRQ_STATUS causes radio's IRQ pin to go low irq_status = radio_spiReadReg(RG_IRQ_STATUS); - + // start of frame event if (irq_status & AT_IRQ_RX_START) { // change state @@ -412,6 +448,6 @@ kick_scheduler_t radio_isr(void) { while(1); } } - + return DO_NOT_KICK_SCHEDULER; } diff --git a/bsp/chips/at86rf233/radio.c b/bsp/chips/at86rf233/radio.c index 018dcb1c65..84a45cc08a 100644 --- a/bsp/chips/at86rf233/radio.c +++ b/bsp/chips/at86rf233/radio.c @@ -1,35 +1,35 @@ /** -* Copyright (c) 2014 Atmel Corporation. All rights reserved. -* -* Redistribution and use in source and binary forms, with or without +* Copyright (c) 2014 Atmel Corporation. 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 +* +* 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. The name of Atmel may not be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* 4. This software may only be redistributed and used in connection with an +* +* 3. The name of Atmel may not be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* 4. This software may only be redistributed and used in connection with an * Atmel microcontroller product. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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 +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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. -* -* -* +* +* +* */ #include "board.h" @@ -47,7 +47,7 @@ typedef struct { radiotimer_capture_cbt startFrame_cb; radiotimer_capture_cbt endFrame_cb; - radio_state_t state; + radio_state_t state; } radio_vars_t; radio_vars_t radio_vars; @@ -72,25 +72,25 @@ void radio_init(void) { // clear variables // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; - + //Check the radio part number. If the H/W interface in not initialized it statys here */ while (trx_reg_read(RG_PART_NUM) != AT86RF233_PART_NUM); - + // configure the radio radio_spiWriteReg(RG_TRX_STATE, CMD_FORCE_TRX_OFF); // turn radio off - + radio_spiWriteReg(RG_IRQ_MASK, (AT_IRQ_RX_START| AT_IRQ_TRX_END)); // tell radio to fire interrupt on TRX_END and RX_START radio_spiReadReg(RG_IRQ_STATUS); // deassert the interrupt pin in case is high radio_spiWriteReg(RG_ANT_DIV, RADIO_CHIP_ANTENNA); // use chip antenna radio_spiWriteReg(RG_TRX_CTRL_1, 0x20); // have the radio calculate CRC //busy wait until radio status is TRX_OFF - + while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != TRX_OFF); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -140,14 +140,18 @@ PORT_TIMER_WIDTH radio_getTimerPeriod(void) { void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - + // configure the radio to the right frequency radio_spiWriteReg(RG_PHY_CC_CCA,0x20+frequency); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { PORT_PIN_RADIO_RESET_LOW(); } @@ -160,11 +164,11 @@ void radio_rfOff(void) { radio_spiWriteReg(RG_TRX_STATE, CMD_FORCE_TRX_OFF); //radio_spiWriteReg(RG_TRX_STATE, CMD_TRX_OFF); while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != TRX_OFF); // busy wait until done - + // wiggle debug pin debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -174,10 +178,10 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + // load packet in TXFIFO radio_spiWriteTxFifo(packet,len); - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -185,15 +189,15 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { void radio_txEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // turn on radio's PLL radio_spiWriteReg(RG_TRX_STATE, CMD_PLL_ON); while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != PLL_ON); // busy wait until done - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -202,11 +206,11 @@ void radio_txNow(void) { PORT_TIMER_WIDTH val; // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + // send packet by pulsing the SLP_TR_CNTL pin PORT_PIN_RADIO_SLP_TR_CNTL_HIGH(); PORT_PIN_RADIO_SLP_TR_CNTL_LOW(); - + // The AT86RF231 does not generate an interrupt when the radio transmits the // SFD, which messes up the MAC state machine. The danger is that, if we leave // this funtion like this, any radio watchdog timer will expire. @@ -226,17 +230,17 @@ void radio_txNow(void) { void radio_rxEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + // put radio in reception mode radio_spiWriteReg(RG_TRX_STATE, CMD_RX_ON); - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // busy wait until radio really listening while((radio_spiReadReg(RG_TRX_STATUS) & 0x1F) != RX_ON); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -252,17 +256,17 @@ void radio_getReceivedFrame(uint8_t* pBufRead, uint8_t* pLqi, uint8_t* pCrc) { uint8_t temp_reg_value; - + //===== crc temp_reg_value = radio_spiReadReg(RG_PHY_RSSI); *pCrc = (temp_reg_value & 0x80)>>7; // msb is whether packet passed CRC - + //===== rssi // as per section 8.5.3 of the AT86RF233, the RSSI is calculate as: // -91 + ED [dBm] temp_reg_value = radio_spiReadReg(RG_PHY_ED_LEVEL); *pRssi = -91 + temp_reg_value; - + //===== packet radio_spiReadRxFifo(pBufRead, pLenRead, @@ -299,10 +303,10 @@ uint8_t radio_spiReadRadioInfo(void) { void radio_spiWriteReg(uint8_t reg_addr, uint8_t reg_setting) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[2]; - + spi_tx_buffer[0] = (0xC0 | reg_addr); // turn address in a 'reg write' address spi_tx_buffer[1] = reg_setting; - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -315,10 +319,10 @@ void radio_spiWriteReg(uint8_t reg_addr, uint8_t reg_setting) { uint8_t radio_spiReadReg(uint8_t reg_addr) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[2]; - + spi_tx_buffer[0] = (0x80 | reg_addr); // turn addess in a 'reg read' address spi_tx_buffer[1] = 0x00; // send a no_operation command just to get the reg value - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -326,7 +330,7 @@ uint8_t radio_spiReadReg(uint8_t reg_addr) { sizeof(spi_rx_buffer), SPI_FIRST, SPI_LAST); - + return spi_rx_buffer[1]; } @@ -336,10 +340,10 @@ uint8_t radio_spiReadReg(uint8_t reg_addr) { void radio_spiWriteTxFifo(uint8_t* bufToWrite, uint8_t lenToWrite) { uint8_t spi_tx_buffer[2]; uint8_t spi_rx_buffer[1+1+127]; // 1B SPI address, 1B length, max. 127B data - + spi_tx_buffer[0] = 0x60; // SPI destination address for TXFIFO spi_tx_buffer[1] = lenToWrite; // length byte - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -347,7 +351,7 @@ void radio_spiWriteTxFifo(uint8_t* bufToWrite, uint8_t lenToWrite) { sizeof(spi_rx_buffer), SPI_FIRST, SPI_NOTLAST); - + spi_txrx(bufToWrite, lenToWrite, SPI_BUFFER, @@ -371,9 +375,9 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, // - *[1B] LQI uint8_t spi_tx_buffer[125]; uint8_t spi_rx_buffer[3]; - + spi_tx_buffer[0] = 0x20; - + // 2 first bytes spi_txrx(spi_tx_buffer, 2, @@ -382,12 +386,12 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, sizeof(spi_rx_buffer), SPI_FIRST, SPI_NOTLAST); - + *pLenRead = spi_rx_buffer[1]; - + if (*pLenRead>2 && *pLenRead<=127) { // valid length - + //read packet spi_txrx(spi_tx_buffer, *pLenRead, @@ -396,7 +400,7 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, 125, SPI_NOTFIRST, SPI_NOTLAST); - + // CRC (2B) and LQI (1B) spi_txrx(spi_tx_buffer, 2+1, @@ -405,12 +409,12 @@ void radio_spiReadRxFifo(uint8_t* pBufRead, 3, SPI_NOTFIRST, SPI_LAST); - + *pLqi = spi_rx_buffer[2]; - + } else { // invalid length - + // read a just byte to close spi spi_txrx(spi_tx_buffer, 1, @@ -435,7 +439,7 @@ kick_scheduler_t radio_isr(void) { // reading IRQ_STATUS causes radio's IRQ pin to go low irq_status = radio_spiReadReg(RG_IRQ_STATUS); - + // start of frame event if (irq_status & AT_IRQ_RX_START) { // change state @@ -462,6 +466,6 @@ kick_scheduler_t radio_isr(void) { while(1); } } - + return DO_NOT_KICK_SCHEDULER; } diff --git a/bsp/chips/cc1101/radio.c b/bsp/chips/cc1101/radio.c index 1b537349fa..ebe8e3b76f 100644 --- a/bsp/chips/cc1101/radio.c +++ b/bsp/chips/cc1101/radio.c @@ -47,17 +47,17 @@ void radio_init(void) { // clear variables memset(&radio_vars, 0, sizeof(radio_vars_t)); - // change state + // change state radio_vars.state = RADIOSTATE_STOPPED; - + // Set SCLK = 1 and SIMO = 0 PORT_PIN_SCLK_HIGH(); - PORT_PIN_SIMO_LOW(); + PORT_PIN_SIMO_LOW(); // Strobe CSn low/high PORT_PIN_CS_LOW(); PORT_PIN_CS_HIGH(); - + // Hold CSn low and high for 40 microsec PORT_PIN_CS_LOW(); delay(40); @@ -74,9 +74,9 @@ void radio_init(void) { radio_reset(); // Wait until SOMI goes low again - while (radio_vars.radioStatusByte.CHIP_RDYn != 0x00); + while (radio_vars.radioStatusByte.CHIP_RDYn != 0x00); - // change state + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -99,11 +99,11 @@ void radio_reset(void) { cc1101_MCSM0_reg_t cc1101_MCSM0_reg; - // global reset + // global reset radio_spiStrobe(CC1101_SRES, &radio_vars.radioStatusByte); // default setting as recommended in datasheet - cc1101_IOCFG0_reg.GDO0_CFG = 63; + cc1101_IOCFG0_reg.GDO0_CFG = 63; cc1101_IOCFG0_reg.GDO0_INV = 0; cc1101_IOCFG0_reg.TEMP_SENSOR_ENABLE = 0; @@ -111,9 +111,9 @@ void radio_reset(void) { &radio_vars.radioStatusByte, *(uint8_t*)&cc1101_IOCFG0_reg); - + // setting packet control - cc1101_PKTCTRL0_reg.LENGTH_CONFIG = 0; // Fixing packet length + cc1101_PKTCTRL0_reg.LENGTH_CONFIG = 0; // Fixing packet length cc1101_PKTCTRL0_reg.CRC_EN = 1; // Enabling CRC calculation cc1101_PKTCTRL0_reg.unused_r0_2 = 0; cc1101_PKTCTRL0_reg.PKT_FORMAT = 0; @@ -125,7 +125,7 @@ void radio_reset(void) { // Setting packet length to 128 bytes cc1101_PKTLEN_reg.PACKET_LENGTH = 128; - + radio_spiWriteReg(CC1101_PKTLEN, &radio_vars.radioStatusByte, *(uint8_t*)&cc1101_PKTLEN_reg); @@ -136,12 +136,12 @@ void radio_reset(void) { cc1101_MDMCFG2_reg.MANCHESTER_EN = 0; cc1101_MDMCFG2_reg.MOD_FORMAT = 0; cc1101_MDMCFG2_reg.DEM_DCFILT_OFF = 0; - + radio_spiWriteReg(CC1101_MDMCFG2, &radio_vars.radioStatusByte, *(uint8_t*)&cc1101_MDMCFG2_reg); - // setting main control state machine + // setting main control state machine cc1101_MCSM0_reg.FS_AUTOCAL = 0; cc1101_MCSM0_reg.PO_TIMEOUT = 1; cc1101_MCSM0_reg.PIN_CTRL_EN = 0; @@ -150,8 +150,8 @@ void radio_reset(void) { radio_spiWriteReg(CC1101_MCSM0, &radio_vars.radioStatusByte, *(uint8_t*)&cc1101_MCSM0_reg); - - + + } //==== RF admin @@ -165,7 +165,7 @@ void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - // setting least significant bits + // setting least significant bits cc1101_FREQ0_reg.FREQ = frequency; radio_spiWriteReg(CC1101_FREQ0, @@ -181,7 +181,7 @@ void radio_setFrequency(uint8_t frequency) { cc1101_FREQ2_reg.FREQ_1 = 0; // always 0 - cc1101_FREQ2_reg.FREQ_2 = 0; + cc1101_FREQ2_reg.FREQ_2 = 0; radio_spiWriteReg(CC1101_FREQ2, &radio_vars.radioStatusByte, @@ -190,8 +190,12 @@ void radio_setFrequency(uint8_t frequency) { // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; - - + + +} + +void radio_setTxPower(int8_t power) { + // TODO } void radio_rfOn(void) { @@ -204,13 +208,13 @@ void radio_rfOff(void) { // calibrates frequency synthesizer and turns it off radio_spiStrobe(CC1101_SCAL, &radio_vars.radioStatusByte); - + debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; - + } //==== TX @@ -218,10 +222,10 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + radio_spiStrobe(CC1101_SFTX, &radio_vars.radioStatusByte); radio_spiWriteTxFifo(&radio_vars.radioStatusByte, packet, len); - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -229,11 +233,11 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { void radio_txEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -242,7 +246,7 @@ void radio_txEnable(void) { void radio_txNow(void) { // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + radio_spiStrobe(CC1101_STX, &radio_vars.radioStatusByte); } @@ -253,22 +257,22 @@ void radio_rxEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + // put radio in reception mode radio_spiStrobe(CC1101_SRX, &radio_vars.radioStatusByte); radio_spiStrobe(CC1101_SFRX, &radio_vars.radioStatusByte); - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // busy wait until radio really listening while (radio_vars.radioStatusByte.STATE == 0x01) { radio_spiStrobe(CC1101_SNOP, &radio_vars.radioStatusByte); } - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -285,7 +289,7 @@ void radio_getReceivedFrame(uint8_t* bufRead, bool* crc) { // read the received packet from the RXFIFO radio_spiReadRxFifo(&radio_vars.radioStatusByte, bufRead, lenRead, maxBufLen); - + // On reception, because of PCKTCTRL.APPEND_STATUS enabled, // we receive : // - [1B] the rssi @@ -300,9 +304,9 @@ void radio_getReceivedFrame(uint8_t* bufRead, void radio_spiStrobe(uint8_t strobe, cc1101_status_t* statusRead) { uint8_t spi_tx_buffer[1]; - + spi_tx_buffer[0] = (CC1101_WRITE_SINGLE | strobe); - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_FIRSTBYTE, @@ -315,11 +319,11 @@ void radio_spiStrobe(uint8_t strobe, cc1101_status_t* statusRead) { void radio_spiWriteReg(uint8_t reg, cc1101_status_t* statusRead, uint8_t regValueToWrite) { uint8_t spi_tx_buffer[3]; - - spi_tx_buffer[0] = (CC1101_WRITE_SINGLE | reg); + + spi_tx_buffer[0] = (CC1101_WRITE_SINGLE | reg); spi_tx_buffer[1] = regValueToWrite/256; spi_tx_buffer[2] = regValueToWrite%256; - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_FIRSTBYTE, @@ -332,11 +336,11 @@ void radio_spiWriteReg(uint8_t reg, cc1101_status_t* statusRead, uint8_t regValu void radio_spiReadReg(uint8_t reg, cc1101_status_t* statusRead, uint8_t* regValueRead) { uint8_t spi_tx_buffer[3]; uint8_t spi_rx_buffer[3]; - + spi_tx_buffer[0] = (CC1101_READ_SINGLE | reg); spi_tx_buffer[1] = 0x00; spi_tx_buffer[2] = 0x00; - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_BUFFER, @@ -344,7 +348,7 @@ void radio_spiReadReg(uint8_t reg, cc1101_status_t* statusRead, uint8_t* regValu sizeof(spi_rx_buffer), SPI_FIRST, SPI_LAST); - + *statusRead = *(cc1101_status_t*)&spi_rx_buffer[0]; *(regValueRead+0) = spi_rx_buffer[2]; *(regValueRead+1) = spi_rx_buffer[1]; @@ -353,11 +357,11 @@ void radio_spiReadReg(uint8_t reg, cc1101_status_t* statusRead, uint8_t* regValu void radio_spiWriteTxFifo(cc1101_status_t* statusRead, uint8_t* bufToWrite, uint8_t len) { uint8_t spi_tx_buffer[2]; - + // step 1. send SPI address and length byte spi_tx_buffer[0] = (CC1101_TX_BURST); spi_tx_buffer[1] = len; - + spi_txrx(spi_tx_buffer, sizeof(spi_tx_buffer), SPI_FIRSTBYTE, @@ -365,7 +369,7 @@ void radio_spiWriteTxFifo(cc1101_status_t* statusRead, uint8_t* bufToWrite, uint 1, SPI_FIRST, SPI_NOTLAST); - + // step 2. send payload spi_txrx(bufToWrite, len, @@ -389,9 +393,9 @@ void radio_spiReadRxFifo(cc1101_status_t* statusRead, // - *[2B] RSSI, CRC and LQI uint8_t spi_tx_buffer[125]; uint8_t spi_rx_buffer[3]; - + spi_tx_buffer[0] = (CC1101_RX_BURST); - + // 2 first bytes spi_txrx(spi_tx_buffer, 2, @@ -400,13 +404,13 @@ void radio_spiReadRxFifo(cc1101_status_t* statusRead, sizeof(spi_rx_buffer), SPI_FIRST, SPI_NOTLAST); - + *statusRead = *(cc1101_status_t*)&spi_rx_buffer[0]; *pLenRead = spi_rx_buffer[1]; - + if (*pLenRead>2 && *pLenRead<=127) { // valid length - + //read packet spi_txrx(spi_tx_buffer, *pLenRead, @@ -418,7 +422,7 @@ void radio_spiReadRxFifo(cc1101_status_t* statusRead, } else { // invalid length - + // read a just byte to close spi spi_txrx(spi_tx_buffer, 1, diff --git a/bsp/chips/cc1200/radio.c b/bsp/chips/cc1200/radio.c index 04d7249d60..28fb40ea36 100644 --- a/bsp/chips/cc1200/radio.c +++ b/bsp/chips/cc1200/radio.c @@ -32,16 +32,16 @@ void radio_init(void) { // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; - + // reset radio radio_reset(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; - + P1SEL &= (~BIT3); // Set P1.3 SEL as GPIO P1DIR &= (~BIT3); // Set P1.3 SEL as Input P1IES |= (BIT3); // Falling Edge @@ -78,8 +78,8 @@ void radio_setEndFrameCb(radiotimer_capture_cbt cb) { //===== reset void radio_reset(void) { - - cc1200_spiStrobe( CC1200_SRES, &radio_vars.radioStatusByte); + + cc1200_spiStrobe( CC1200_SRES, &radio_vars.radioStatusByte); } //===== timer @@ -103,11 +103,15 @@ uint16_t radio_getTimerPeriod(void) { //===== RF admin void radio_setFrequency(uint8_t frequency) { - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } +void radio_setTxPower(int8_t power) { + // TODO +} + void radio_rfOn(void) { uint8_t marcState; // Calibrate radio @@ -120,18 +124,18 @@ void radio_rfOn(void) { } void radio_rfOff(void) { - + // change state radio_vars.state = RADIOSTATE_TURNING_OFF; - + cc1200_spiStrobe(CC1200_SPWD, &radio_vars.radioStatusByte); // poipoipoi wait until off cc1200_spiStrobe(CC1200_SXOFF, &radio_vars.radioStatusByte); - + // wiggle debug pin debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -141,7 +145,7 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { //test 802.15.4g PHR. This has to be done by the MAC layer uint8_t PHR[2]; - uint8_t aux; + uint8_t aux; if (len<2048){ PHR[0] = len/256; @@ -160,7 +164,7 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { }else{ //error } - + } @@ -171,7 +175,7 @@ void radio_txEnable(void) { // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -179,7 +183,7 @@ void radio_txEnable(void) { void radio_txNow(void) { // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + cc1200_spiStrobe( CC1200_STX, &radio_vars.radioStatusByte); cc1200_spiStrobe( CC1200_SNOP, &radio_vars.radioStatusByte); while(radio_vars.state != RADIOSTATE_TXRX_DONE); @@ -191,10 +195,10 @@ void radio_rxEnable(void) { uint8_t aux; // change state radio_vars.state = RADIOSTATE_ENABLING_RX; - + //calibrate ROCos cc1200_spiReadReg(CC1200_WOR_CFG0, &radio_vars.radioStatusByte, &aux); - aux = (aux & 0xF9) | (0x02 << 1); + aux = (aux & 0xF9) | (0x02 << 1); // Write new register value cc1200_spiWriteReg(CC1200_WOR_CFG0, &radio_vars.radioStatusByte, aux); // Strobe IDLE to calibrate the RCOSC @@ -202,14 +206,14 @@ void radio_rxEnable(void) { // Disable RC calibration aux = (aux & 0xF9) | (0x00 << 1); cc1200_spiWriteReg(CC1200_WOR_CFG0, &radio_vars.radioStatusByte, aux); - + //put radio in reception mode cc1200_spiStrobe(CC1200_SWOR, &radio_vars.radioStatusByte); //sniffer mode - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // change state radio_vars.state = RADIOSTATE_LISTENING; } @@ -228,12 +232,12 @@ void radio_getReceivedFrame( ) { uint8_t marcStatus1; cc1200_spiReadReg( CC1200_MARC_STATUS1, &radio_vars.radioStatusByte, &marcStatus1); - - //read FIFO length + + //read FIFO length cc1200_spiReadReg(CC1200_NUM_RXBYTES, &radio_vars.radioStatusByte, lenRead); // read the received packet from the RXFIFO cc1200_spiReadRxFifo(&radio_vars.radioStatusByte, bufRead, lenRead, maxBufLen); - + //TODO // On reception, the CC1200 replaces the // received CRC by: @@ -242,7 +246,7 @@ void radio_getReceivedFrame( //*rssi = *(bufRead+*lenRead-1); //*crc = ((*(bufRead+*lenRead))&0x80)>>7; //*lqi = (*(bufRead+*lenRead))&0x7f; - //clean RX FIFO + //clean RX FIFO cc1200_spiStrobe( CC1200_SFRX, &radio_vars.radioStatusByte); //put radio in reception mode cc1200_spiStrobe(CC1200_SWOR, &radio_vars.radioStatusByte); @@ -302,6 +306,6 @@ kick_scheduler_t radio_isr(void) { } break; } - + return DO_NOT_KICK_SCHEDULER; } diff --git a/bsp/chips/cc2420/radio.c b/bsp/chips/cc2420/radio.c index cd0c76f18b..07487d0b16 100644 --- a/bsp/chips/cc2420/radio.c +++ b/bsp/chips/cc2420/radio.c @@ -23,6 +23,18 @@ typedef struct { radio_vars_t radio_vars; +/* TX Power dBm lookup table. Values from CC2420 datasheet */ +static const radio_output_power_config_t cc2420_output_power[] = { + { 0, 31 }, + { -1, 27 }, + { -3, 23 }, + { -5, 19 }, + { -7, 15 }, + { -10, 11 }, + { -15, 7 }, + { -25, 3 }, +}; + //=========================== public ========================================== //===== admin @@ -30,16 +42,16 @@ radio_vars_t radio_vars; void radio_init(void) { // clear variables memset(&radio_vars,0,sizeof(radio_vars_t)); - + // change state radio_vars.state = RADIOSTATE_STOPPED; - + // reset radio radio_reset(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; - + } void radio_setStartFrameCb(radio_capture_cbt cb) { @@ -57,19 +69,19 @@ void radio_reset(void) { cc2420_MDMCTRL0_reg_t cc2420_MDMCTRL0_reg; cc2420_TXCTRL_reg_t cc2420_TXCTRL_reg; cc2420_RXCTRL1_reg_t cc2420_RXCTRL1_reg; - + // set radio VREG pin high PORT_PIN_RADIO_VREG_HIGH(); for (delay=0xffff;delay>0;delay--); // max. VREG start-up time is 0.6ms - + // set radio RESET pin low PORT_PIN_RADIO_RESET_LOW(); for (delay=0xffff;delay>0;delay--); - + // set radio RESET pin high PORT_PIN_RADIO_RESET_HIGH(); for (delay=0xffff;delay>0;delay--); - + // disable address recognition cc2420_MDMCTRL0_reg.PREAMBLE_LENGTH = 2; // 3 leading zero's (IEEE802.15.4 compliant) cc2420_MDMCTRL0_reg.AUTOACK = 0; @@ -85,7 +97,7 @@ void radio_reset(void) { &radio_vars.radioStatusByte, *(uint16_t*)&cc2420_MDMCTRL0_reg ); - + // speed up time to TX cc2420_TXCTRL_reg.PA_LEVEL = 31;// max. TX power (~0dBm) cc2420_TXCTRL_reg.reserved_w1 = 1; @@ -99,7 +111,7 @@ void radio_reset(void) { &radio_vars.radioStatusByte, *(uint16_t*)&cc2420_TXCTRL_reg ); - + // apply correction recommended in datasheet cc2420_RXCTRL1_reg.RXMIX_CURRENT = 2; cc2420_RXCTRL1_reg.RXMIX_VCM = 1; @@ -123,10 +135,10 @@ void radio_reset(void) { void radio_setFrequency(uint8_t frequency) { cc2420_FSCTRL_reg_t cc2420_FSCTRL_reg; - + // change state radio_vars.state = RADIOSTATE_SETTING_FREQUENCY; - + cc2420_FSCTRL_reg.FREQ = frequency-11; cc2420_FSCTRL_reg.FREQ *= 5; cc2420_FSCTRL_reg.FREQ += 357; @@ -135,18 +147,49 @@ void radio_setFrequency(uint8_t frequency) { cc2420_FSCTRL_reg.CAL_RUNNING = 0; cc2420_FSCTRL_reg.CAL_DONE = 0; cc2420_FSCTRL_reg.LOCK_THR = 1; - + cc2420_spiWriteReg( CC2420_FSCTRL_ADDR, &radio_vars.radioStatusByte, *(uint16_t*)&cc2420_FSCTRL_reg ); - + // change state radio_vars.state = RADIOSTATE_FREQUENCY_SET; } -void radio_rfOn(void) { +// configure the radio to output at least @power dBm, or the maximum available power +void radio_setTxPower(int8_t power) { + cc2420_TXCTRL_reg_t cc2420_TXCTRL_reg; + uint8_t i; + uint8_t reg_val; + + // loop to find at-least :power: dBm in the look-up table + // if the max output power of a chip is higher than :power:, we configure the maximum + reg_val = cc2420_output_power[0].register_val; + for(i = sizeof(cc2420_output_power) / sizeof(radio_output_power_config_t) - 1; i >= 0; --i) { + if(power <= cc2420_output_power[i].power_dbm) { + reg_val = cc2420_output_power[i].register_val; + break; + } + } + + // speed up time to TX + cc2420_TXCTRL_reg.PA_LEVEL = reg_val; + cc2420_TXCTRL_reg.reserved_w1 = 1; + cc2420_TXCTRL_reg.PA_CURRENT = 3; + cc2420_TXCTRL_reg.TXMIX_CURRENT = 0; + cc2420_TXCTRL_reg.TXMIX_CAP_ARRAY = 0; + cc2420_TXCTRL_reg.TX_TURNAROUND = 0; // faster STXON->SFD timing (128us) + cc2420_TXCTRL_reg.TXMIXBUF_CUR = 2; + cc2420_spiWriteReg( + CC2420_TXCTRL_ADDR, + &radio_vars.radioStatusByte, + *(uint16_t*)&cc2420_TXCTRL_reg + ); +} + +void radio_rfOn(void) { cc2420_spiStrobe(CC2420_SXOSCON, &radio_vars.radioStatusByte); while (radio_vars.radioStatusByte.xosc16m_stable==0) { cc2420_spiStrobe(CC2420_SNOP, &radio_vars.radioStatusByte); @@ -154,17 +197,17 @@ void radio_rfOn(void) { } void radio_rfOff(void) { - + // change state radio_vars.state = RADIOSTATE_TURNING_OFF; - + cc2420_spiStrobe(CC2420_SRFOFF, &radio_vars.radioStatusByte); // poipoipoi wait until off - + // wiggle debug pin debugpins_radio_clr(); leds_radio_off(); - + // change state radio_vars.state = RADIOSTATE_RFOFF; } @@ -174,10 +217,10 @@ void radio_rfOff(void) { void radio_loadPacket(uint8_t* packet, uint16_t len) { // change state radio_vars.state = RADIOSTATE_LOADING_PACKET; - + cc2420_spiStrobe(CC2420_SFLUSHTX, &radio_vars.radioStatusByte); cc2420_spiWriteFifo(&radio_vars.radioStatusByte, packet, len, CC2420_TXFIFO_ADDR); - + // change state radio_vars.state = RADIOSTATE_PACKET_LOADED; } @@ -185,13 +228,13 @@ void radio_loadPacket(uint8_t* packet, uint16_t len) { void radio_txEnable(void) { // change state radio_vars.state = RADIOSTATE_ENABLING_TX; - + // wiggle debug pin debugpins_radio_set(); leds_radio_on(); - + // I don't fully understand how the CC2420_STXCA the can be used here. - + // change state radio_vars.state = RADIOSTATE_TX_ENABLED; } @@ -199,7 +242,7 @@ void radio_txEnable(void) { void radio_txNow(void) { // change state radio_vars.state = RADIOSTATE_TRANSMITTING; - + cc2420_spiStrobe(CC2420_STXON, &radio_vars.radioStatusByte); } @@ -236,10 +279,10 @@ void radio_getReceivedFrame( uint8_t* lqi, bool* crc ) { - + // read the received packet from the RXFIFO cc2420_spiReadRxFifo(&radio_vars.radioStatusByte, bufRead, lenRead, maxBufLen); - + // On reception, when MODEMCTRL0.AUTOCRC is set, the CC2420 replaces the // received CRC by: // - [1B] the rssi, a signed value. The actual value in dBm is that - 45. diff --git a/drivers/common/openserial.c b/drivers/common/openserial.c index d39634c164..591d1921b7 100644 --- a/drivers/common/openserial.c +++ b/drivers/common/openserial.c @@ -28,6 +28,9 @@ #include "icmpv6echo.h" #include "msf.h" #include "debugpins.h" +#include "radio.h" +#include "packetfunctions.h" +#include "eui64.h" //=========================== variables ======================================= @@ -122,6 +125,11 @@ void openserial_register(openserial_rsvpt* rsvp) { openserial_vars.registeredCmd = rsvp; } +// register a callback for sendPacket command +void openserial_registerSendPacketCb(callbackSendPacket_cbt cb) { + openserial_vars.callbackSendPacket = cb; +} + //===== transmitting owerror_t openserial_printStatus( @@ -262,6 +270,48 @@ owerror_t openserial_printSniffedPacket(uint8_t* buffer, uint8_t length, uint8_t return E_SUCCESS; } +owerror_t openserial_printBenchmark( + uint8_t statusElement, + uint8_t* buffer, + uint8_t length +) { + uint8_t i; + uint8_t asn[5]; + uint8_t source[8]; + + // retrieve ASN + ieee154e_getAsn(asn); + eui64_get(source); + + outputHdlcOpen(); + + outputHdlcWrite(SERFRAME_MOTE2PC_BENCHMARK); + + for (i=0;i<8;i++){ + outputHdlcWrite(source[i]); + } + + // event + outputHdlcWrite(statusElement); + + // timestamp + outputHdlcWrite(asn[0]); + outputHdlcWrite(asn[1]); + outputHdlcWrite(asn[2]); + outputHdlcWrite(asn[3]); + outputHdlcWrite(asn[4]); + + for (i=0;imote #define SERFRAME_PC2MOTE_SETROOT ((uint8_t)'R') @@ -80,14 +81,18 @@ enum { COMMAND_SET_UINJECTPERIOD = 17, COMMAND_SET_ECHO_REPLY_STATUS = 18, COMMAND_SET_JOIN_KEY = 19, - COMMAND_MAX = 20, + COMMAND_SET_TX_POWER = 20, + COMMAND_SEND_PACKET = 21, + COMMAND_MAX = 22, }; + //=========================== variables ======================================= //=========================== prototypes ====================================== typedef void (*openserial_cbt)(void); +typedef void (*callbackSendPacket_cbt)(uint8_t *buf, uint8_t bufLen); typedef struct _openserial_rsvpt { uint8_t cmdId; ///< serial command (e.g. 'B') @@ -103,6 +108,7 @@ typedef struct { openserial_rsvpt* registeredCmd; uint8_t reset_timerId; uint8_t debugPrint_timerId; + callbackSendPacket_cbt callbackSendPacket; // input uint8_t inputBuf[SERIAL_INPUT_BUFFER_SIZE]; uint8_t inputBufFillLevel; @@ -121,6 +127,7 @@ typedef struct { // admin void openserial_init(void); void openserial_register(openserial_rsvpt* rsvp); +void openserial_registerSendPacketCb(callbackSendPacket_cbt cb); // transmitting owerror_t openserial_printStatus( @@ -155,6 +162,11 @@ owerror_t openserial_printSniffedPacket( uint8_t length, uint8_t channel ); +owerror_t openserial_printBenchmark( + uint8_t statusElement, + uint8_t* buffer, + uint8_t length +); void task_openserial_debugPrint(void); diff --git a/inc/opendefs.h b/inc/opendefs.h index 69909ad714..e10c450a7e 100644 --- a/inc/opendefs.h +++ b/inc/opendefs.h @@ -107,9 +107,20 @@ enum { STATUS_BACKOFF = 7, STATUS_QUEUE = 8, STATUS_NEIGHBORS = 9, - STATUS_KAPERIOD = 10, - STATUS_JOINED = 11, - STATUS_MAX = 12, + STATUS_KAPERIOD = 10, + STATUS_MAX = 11, +}; + +// benchmarking events +enum { + BENCHMARK_EVENT_PACKETSENT = 0, + BENCHMARK_EVENT_PACKETRECEIVED = 1, + BENCHMARK_EVENT_SYNCHRONIZED = 2, + BENCHMARK_EVENT_JOINED = 3, + BENCHMARK_EVENT_BANDWIDTHASSIGNED = 4, + BENCHMARK_EVENT_PACKETSENTDAGROOT = 5, // special event to log the asn when dag root sent the packet + BENCHMARK_EVENT_DESYNCHRONIZED = 6, + BENCHMARK_EVENT_MAX = 7, }; //component identifiers @@ -172,6 +183,7 @@ enum { COMPONENT_UEXPIRATION = 0x27, COMPONENT_UMONITOR = 0x28, COMPONENT_CINFRARED = 0x29, + COMPONENT_CBENCHMARK = 0x2a, }; /** @@ -322,6 +334,7 @@ typedef struct { open_addr_t l3_destinationAdd; // 128b IPv6 destination (down stack) open_addr_t l3_sourceAdd; // 128b IPv6 source address bool l3_useSourceRouting; // TRUE when the packet goes downstream + uint8_t l3_hopLimit; // Value of the hop limit field //l2 owerror_t l2_sendDoneError; // outcome of trying to send this packet open_addr_t l2_nextORpreviousHop; // 64b IEEE802.15.4 next (down stack) or previous (up) hop address diff --git a/openapps/SConscript b/openapps/SConscript index c342fa0824..c9cf2eb7c9 100644 --- a/openapps/SConscript +++ b/openapps/SConscript @@ -21,6 +21,7 @@ if localEnv['board']=='python': 'rrt', 'cexample', 'cstorm', + 'cbenchmark', ] # source files that should be included in the build other than diff --git a/openapps/cbenchmark/cbenchmark.c b/openapps/cbenchmark/cbenchmark.c new file mode 100644 index 0000000000..6f78352af8 --- /dev/null +++ b/openapps/cbenchmark/cbenchmark.c @@ -0,0 +1,309 @@ +/** +\brief App that generates packets as instructed over serial. +*/ + +#include "opendefs.h" +#include "opencoap.h" +#include "opentimers.h" +#include "openqueue.h" +#include "packetfunctions.h" +#include "openserial.h" +#include "openrandom.h" +#include "scheduler.h" +#include "idmanager.h" +#include "eui64.h" +#include "neighbors.h" +#include "cbenchmark.h" +#include "IEEE802154E.h" +#include "iphc.h" + +//=========================== defines ========================================= + +const uint8_t cbenchmark_path0[] = "b"; + +//=========================== variables ======================================= + +cbenchmark_vars_t cbenchmark_vars; + +//=========================== prototypes ====================================== + +owerror_t cbenchmark_receive(OpenQueueEntry_t* msg, + coap_header_iht* coap_header, + coap_option_iht* coap_incomingOptions, + coap_option_iht* coap_outgoingOptions, + uint8_t* coap_outgoingOptionsLen); +void cbenchmark_sendDone(OpenQueueEntry_t* msg, owerror_t error); +void cbenchmark_sendPacket(uint8_t *, uint8_t); +owerror_t cbenchmark_parse_sendPacket(uint8_t *buf, uint8_t bufLen, cbenchmark_sendPacket_t *request); +void cbenchmark_printSerial_packetSentReceived(uint8_t eventType, uint8_t *packetToken, open_addr_t *dest, uint8_t hopLimit); +void cbenchmark_sendPacket_task_cb(void); + +//=========================== public ========================================== + +void cbenchmark_init() { + // prepare the resource descriptor for the /b path + cbenchmark_vars.desc.path0len = sizeof(cbenchmark_path0)-1; + cbenchmark_vars.desc.path0val = (uint8_t*)(&cbenchmark_path0); + cbenchmark_vars.desc.path1len = 0; + cbenchmark_vars.desc.path1val = NULL; + cbenchmark_vars.desc.componentID = COMPONENT_CBENCHMARK; + cbenchmark_vars.desc.securityContext = NULL; + cbenchmark_vars.desc.discoverable = TRUE; + cbenchmark_vars.desc.callbackRx = &cbenchmark_receive; + cbenchmark_vars.desc.callbackSendDone = &cbenchmark_sendDone; + + cbenchmark_vars.noResponse = 26; // RFC7967 flag to suppress all responses + + opencoap_register(&cbenchmark_vars.desc); + + // Register sendPacket handler callback + openserial_registerSendPacketCb(&cbenchmark_sendPacket); + + cbenchmark_vars.timerId = opentimers_create(TIMER_GENERAL_PURPOSE, TASKPRIO_COAP); + +} + +//=========================== private ========================================= +owerror_t cbenchmark_receive(OpenQueueEntry_t* msg, + coap_header_iht* coap_header, + coap_option_iht* coap_incomingOptions, + coap_option_iht* coap_outgoingOptions, + uint8_t* coap_outgoingOptionsLen) { + + uint8_t token[5]; + open_addr_t source; + open_addr_t prefix; // dummy + bool noResponse; + uint8_t i; + + // get the source of the packet + packetfunctions_ip128bToMac64b(&msg->l3_sourceAdd, &prefix, &source); + + // check if this is a request + if (coap_header->Code==COAP_CODE_REQ_POST) { + // sanity check + if (msg->length < 5) { + // log formatting error + openserial_printError( + COMPONENT_CBENCHMARK, + ERR_MSG_UNKNOWN_TYPE, + (errorparameter_t)msg->length, + (errorparameter_t)0 + ); + return E_FAIL; + } + + // toss everything but the last 5 bytes + packetfunctions_tossHeader(msg, msg->length - 5); + + // get the token from the remaining 5 bytes of payload + memcpy(token, msg->payload, 5); + + // generate packetReceived event for this request + cbenchmark_printSerial_packetSentReceived(BENCHMARK_EVENT_PACKETRECEIVED, token, &source, msg->l3_hopLimit); + + // now, construct the ACK response (if any) + msg->payload = &(msg->packet[127]); + msg->length = 0; + coap_header->Code = COAP_CODE_RESP_CHANGED; + + // now check whether an ACK is needed in order to generate the packetSent event corresponding to it + noResponse = FALSE; + i = 0; + while(coap_incomingOptions[i].type != COAP_OPTION_NONE) { + if (coap_incomingOptions[i].type == COAP_OPTION_NUM_NORESPONSE) { + noResponse = TRUE; + break; + } + i++; + } + + if (!noResponse) { + // the token in the ACK is the one from the request with the last byte incremented by one + token[CBENCHMARK_PACKETTOKEN_LEN-1]++; + packetfunctions_reserveHeaderSize(msg, CBENCHMARK_PACKETTOKEN_LEN); + memcpy(msg->payload, token, CBENCHMARK_PACKETTOKEN_LEN); + + // generate the packetSent event for the response + cbenchmark_printSerial_packetSentReceived(BENCHMARK_EVENT_PACKETSENT, token, &source, IPHC_DEFAULT_HOP_LIMIT); + } + } + else { // this is a response + // assert that the response is exactly the length of the packet token, otherwise something is wrong + if (msg->length != CBENCHMARK_PACKETTOKEN_LEN) { + // log formatting error + openserial_printError( + COMPONENT_CBENCHMARK, + ERR_MSG_UNKNOWN_TYPE, + (errorparameter_t)msg->length, + (errorparameter_t)0 + ); + return E_FAIL; + } + // generate the packetReceived event for the response + cbenchmark_printSerial_packetSentReceived(BENCHMARK_EVENT_PACKETRECEIVED, msg->payload, &source, msg->l3_hopLimit); + } + return E_SUCCESS; +} + +void cbenchmark_sendDone(OpenQueueEntry_t* msg, owerror_t error) { + openqueue_freePacketBuffer(msg); +} + +void cbenchmark_sendPacket(uint8_t *buf, uint8_t bufLen) { + memcpy(cbenchmark_vars.cmdBuf, buf, bufLen); + cbenchmark_vars.cmdBufLen = bufLen; + // push task + scheduler_push_task(cbenchmark_sendPacket_task_cb,TASKPRIO_COAP); +} + +void cbenchmark_sendPacket_task_cb(void) { + + OpenQueueEntry_t* pkt; + cbenchmark_sendPacket_t request; + coap_option_iht options[2]; + owerror_t outcome; + uint8_t numOptions; + uint8_t i; + uint8_t timestamp[5]; + + numOptions = 0; + i = 0; + + if (cbenchmark_parse_sendPacket(cbenchmark_vars.cmdBuf, cbenchmark_vars.cmdBufLen, &request) != E_SUCCESS) { + return; + } + else { + // success, invalidate the common buffer + cbenchmark_vars.cmdBufLen = 0; + } + + + for (i = 0; i < request.numPackets; i++) { + // create a CoAP packet + pkt = openqueue_getFreePacketBuffer(COMPONENT_CBENCHMARK); + if (pkt==NULL) { + openserial_printError( + COMPONENT_CBENCHMARK, + ERR_NO_FREE_PACKET_BUFFER, + (errorparameter_t)0, + (errorparameter_t)0 + ); + return; + } + + // take ownership over that packet + pkt->creator = COMPONENT_CBENCHMARK; + pkt->owner = COMPONENT_CBENCHMARK; + + // location-path option + options[numOptions].type = COAP_OPTION_NUM_URIPATH; + options[numOptions].length = sizeof(cbenchmark_path0)-1; + options[numOptions].pValue = (uint8_t *)cbenchmark_path0; + numOptions++; + + // confirmable semantics of openbenchmark denotes whether there is an application-layer + // acknowledgment going back. we implement this behavior by sending or not the CoAP response + if (!request.con) { + // to avoid the server generating a response, we use the No Response option specified + // in RFC7967 + options[numOptions].type = COAP_OPTION_NUM_NORESPONSE; + options[numOptions].length = 1; + options[numOptions].pValue = &cbenchmark_vars.noResponse; + numOptions++; + } + + // metadata + pkt->l4_destination_port = CBENCHMARK_OPENBENCHMARK_COAP_PORT; + // construct destination address + packetfunctions_mac64bToIp128b(idmanager_getMyID(ADDR_PREFIX), &request.dest, &(pkt->l3_destinationAdd)); + + // copy the token as given by OpenBenchmark + packetfunctions_reserveHeaderSize(pkt, CBENCHMARK_PACKETTOKEN_LEN); + memcpy(pkt->payload, &request.token, CBENCHMARK_PACKETTOKEN_LEN); + + // set the first byte of the token to packet counter + pkt->payload[0] = i; + + // zero-out the payload + packetfunctions_reserveHeaderSize(pkt, request.payloadLen); + memset(pkt->payload, 0x00, request.payloadLen); + + // get the current ASN + ieee154e_getAsn(timestamp); + + // send + outcome = opencoap_send( + pkt, + COAP_TYPE_NON, + COAP_CODE_REQ_POST, + 1, // CoAP token len + options, + numOptions, + &cbenchmark_vars.desc + ); + + // avoid overflowing the queue if fails + if (outcome==E_FAIL) { + openqueue_freePacketBuffer(pkt); + openserial_printError( + COMPONENT_CBENCHMARK, + ERR_NO_FREE_PACKET_BUFFER, + (errorparameter_t)0, + (errorparameter_t)0 + ); + } + else { + cbenchmark_printSerial_packetSentReceived(BENCHMARK_EVENT_PACKETSENT, request.token, &request.dest, IPHC_DEFAULT_HOP_LIMIT); + } + } +} + +owerror_t cbenchmark_parse_sendPacket(uint8_t *buf, uint8_t bufLen, cbenchmark_sendPacket_t *request) { + + uint8_t *tmp; + + if (bufLen != 16) { + return E_FAIL; + } + + tmp = buf; + + // parse the command payload + // EUI64 (8B) || CON (1B) || NUMPACKETS (1B) || TOKEN (4B) || PAYLOADLEN (1B) + packetfunctions_readAddress(buf, ADDR_64B, &request->dest, FALSE); + tmp += 8; // skip 8 bytes for EUI-64 + + request->con = (bool) *tmp; + tmp++; // skip 1 byte for CON + + request->numPackets = *tmp; + tmp++; // skip 1 byte for number of packets in the burst + + memcpy(request->token, tmp, CBENCHMARK_PACKETTOKEN_LEN); + tmp += CBENCHMARK_PACKETTOKEN_LEN; // skip token len bytes for token + + request->payloadLen = *tmp; + tmp++; + + return E_SUCCESS; +} + +void cbenchmark_printSerial_packetSentReceived(uint8_t eventType, uint8_t *packetToken, open_addr_t *dest, uint8_t hopLimit) { + uint8_t output[5 + CBENCHMARK_PACKETTOKEN_LEN + LENGTH_ADDR64b + 1]; + uint8_t *tmp; + + tmp = output; + + memcpy(tmp, packetToken, CBENCHMARK_PACKETTOKEN_LEN); + tmp += CBENCHMARK_PACKETTOKEN_LEN; + + memcpy(tmp, dest->addr_64b, LENGTH_ADDR64b); + tmp += LENGTH_ADDR64b; + + *tmp = hopLimit; + tmp++; + + openserial_printBenchmark(eventType, output, tmp - output); +} + diff --git a/openapps/cbenchmark/cbenchmark.h b/openapps/cbenchmark/cbenchmark.h new file mode 100644 index 0000000000..f46af1dfb4 --- /dev/null +++ b/openapps/cbenchmark/cbenchmark.h @@ -0,0 +1,43 @@ +#ifndef __CBENCHMARK_H +#define __CBENCHMARK_H + +/** +\addtogroup AppUdp +\{ +\addtogroup cbenchmark +\{ +*/ +#include "opendefs.h" +//=========================== define ========================================== +#define CBENCHMARK_PACKETTOKEN_LEN 5 +#define CBENCHMARK_OPENBENCHMARK_COAP_PORT 5683 +//=========================== typedef ========================================= + +typedef struct { + coap_resource_desc_t desc; + opentimers_id_t timerId; + uint8_t noResponse; + uint8_t cmdBuf[16]; + uint8_t cmdBufLen; +} cbenchmark_vars_t; + +typedef struct { + open_addr_t dest; + bool con; + uint8_t numPackets; + uint8_t token[CBENCHMARK_PACKETTOKEN_LEN]; + uint8_t payloadLen; +} cbenchmark_sendPacket_t; + +//=========================== variables ======================================= + +//=========================== prototypes ====================================== + +void cbenchmark_init(void); + +/** +\} +\} +*/ + +#endif diff --git a/openapps/cjoin/cjoin.c b/openapps/cjoin/cjoin.c index 2d87372fdb..97b2228ec7 100644 --- a/openapps/cjoin/cjoin.c +++ b/openapps/cjoin/cjoin.c @@ -21,7 +21,8 @@ //=========================== defines ========================================= /// inter-packet period (in ms) -#define TIMEOUT 60000 +#define RETRANSMISSION_TIMEOUT 60000 +#define POLLING_PERIOD 1000 const uint8_t cjoin_path0[] = "j"; @@ -107,13 +108,11 @@ void cjoin_init_security_context(void) { } void cjoin_schedule(void) { - uint16_t delay; if (cjoin_getIsJoined() == FALSE) { - delay = openrandom_get16b(); opentimers_scheduleIn(cjoin_vars.timerId, - (uint32_t) delay, // random wait from 0 to 65535ms + (uint32_t) POLLING_PERIOD, TIME_MS, TIMER_PERIODIC, cjoin_timer_cb @@ -166,7 +165,7 @@ void cjoin_retransmission_cb(opentimers_id_t id) { // task mode by opentimer already opentimers_scheduleIn( cjoin_vars.timerId, - (uint32_t) TIMEOUT, + (uint32_t) RETRANSMISSION_TIMEOUT, TIME_MS, TIMER_ONESHOT, cjoin_retransmission_cb @@ -219,7 +218,7 @@ void cjoin_task_cb(void) { // arm the retransmission timer opentimers_scheduleIn( cjoin_vars.timerId, - (uint32_t) TIMEOUT, + (uint32_t) RETRANSMISSION_TIMEOUT, TIME_MS, TIMER_ONESHOT, cjoin_retransmission_cb @@ -331,8 +330,6 @@ bool cjoin_getIsJoined(void) { } void cjoin_setIsJoined(bool newValue) { - uint8_t array[5]; - asn_t joinAsn; if (cjoin_vars.isJoined == newValue) { return; @@ -340,19 +337,13 @@ void cjoin_setIsJoined(bool newValue) { cjoin_vars.isJoined = newValue; - // Update Join ASN value - ieee154e_getAsn(array); - joinAsn.bytes0and1 = ((uint16_t) array[1] << 8) | ((uint16_t) array[0]); - joinAsn.bytes2and3 = ((uint16_t) array[3] << 8) | ((uint16_t) array[2]); - joinAsn.byte4 = array[4]; - - idmanager_setJoinAsn(&joinAsn); - if (newValue == TRUE) { // log the info openserial_printInfo(COMPONENT_CJOIN, ERR_JOINED, (errorparameter_t)0, (errorparameter_t)0); + + openserial_printBenchmark(BENCHMARK_EVENT_JOINED, NULL, 0); } } diff --git a/openapps/openapps.c b/openapps/openapps.c index 3124d0c610..2d8a956d56 100644 --- a/openapps/openapps.c +++ b/openapps/openapps.c @@ -12,6 +12,7 @@ #include "cinfo.h" #include "cleds.h" #include "cjoin.h" +#include "cbenchmark.h" #include "cwellknown.h" #include "rrt.h" // UDP @@ -39,6 +40,7 @@ void openapps_init(void) { cleds__init(); cjoin_init(); cwellknown_init(); + cbenchmark_init(); //rrt_init(); // UDP diff --git a/openapps/opencoap/opencoap.c b/openapps/opencoap/opencoap.c index 9402cbf270..ba6b0d87db 100644 --- a/openapps/opencoap/opencoap.c +++ b/openapps/opencoap/opencoap.c @@ -114,6 +114,7 @@ void opencoap_receive(OpenQueueEntry_t* msg) { coap_option_iht* objectSecurity; coap_option_iht* proxyScheme; coap_option_iht* statelessProxy; + bool noResponse; // RFC7967 flag supressing all responses uint16_t rcvdSequenceNumber; uint8_t* rcvdKid; uint8_t rcvdKidLen; @@ -183,6 +184,14 @@ void opencoap_receive(OpenQueueEntry_t* msg) { return; } + //== No Response option + option_count = opencoap_find_option(coap_incomingOptions, coap_incomingOptionsLen, COAP_OPTION_NUM_NORESPONSE, &option_index); + if(option_count >= 1) { + noResponse = TRUE; + } else { + noResponse = FALSE; + } + //== Proxy Scheme option option_count = opencoap_find_option(coap_incomingOptions, coap_incomingOptionsLen, COAP_OPTION_NUM_PROXYSCHEME, &option_index); if(option_count >= 1) { @@ -425,6 +434,15 @@ void opencoap_receive(OpenQueueEntry_t* msg) { // call the resource's callback outcome = temp_desc->callbackRx(msg,&coap_header,&coap_incomingOptions[0], coap_outgoingOptions, &coap_outgoingOptionsLen); + + if (noResponse) { + // free the received packet + openqueue_freePacketBuffer(msg); + // stop here: we will not respond to a request with No Response option set + // TODO implement noResponse granularity as per RFC7967 + return; + } + if (outcome == E_FAIL) { securityReturnCode = COAP_CODE_RESP_METHODNOTALLOWED; } @@ -771,6 +789,7 @@ coap_option_class_t opencoap_get_option_class(coap_option_t type) { case COAP_OPTION_NUM_URIQUERY: case COAP_OPTION_NUM_ACCEPT: case COAP_OPTION_NUM_LOCATIONQUERY: + case COAP_OPTION_NUM_NORESPONSE: return COAP_OPTION_CLASS_E; // class I options none supported diff --git a/openapps/opencoap/opencoap.h b/openapps/opencoap/opencoap.h index 407ccb008c..b0d591348a 100644 --- a/openapps/opencoap/opencoap.h +++ b/openapps/opencoap/opencoap.h @@ -19,8 +19,8 @@ static const uint8_t ipAddr_ipsoRD[] = {0x26, 0x07, 0xf7, 0x40, 0x00, 0x00, 0 static const uint8_t ipAddr_motesEecs[] = {0x20, 0x01, 0x04, 0x70, 0x00, 0x66, 0x00, 0x19, \ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}; static const uint8_t ipAddr_local[] = {0x26, 0x07, 0xf1, 0x40, 0x04, 0x00, 0x10, 0x36, \ - 0x4d, 0xcd, 0xab, 0x54, 0x81, 0x99, 0xc1, 0xf7}; - + 0x4d, 0xcd, 0xab, 0x54, 0x81, 0x99, 0xc1, 0xf7}; + static const uint8_t ipAddr_motedata[] = {0x20, 0x01, 0x04, 0x70, 0x00, 0x66, 0x00, 0x17, \ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}; @@ -115,6 +115,7 @@ typedef enum { COAP_OPTION_NUM_PROXYSCHEME = 39, COAP_OPTION_NUM_OBJECTSECURITY = 21, COAP_OPTION_NUM_STATELESSPROXY = 40, + COAP_OPTION_NUM_NORESPONSE = 258, // RFC7967 } coap_option_t; typedef enum { @@ -160,18 +161,18 @@ typedef struct { typedef struct { // common context uint8_t aeadAlgorithm; - // sender context + // sender context uint8_t senderID[OSCOAP_MAX_ID_LEN]; uint8_t senderIDLen; uint8_t senderKey[AES_CCM_16_64_128_KEY_LEN]; uint8_t senderIV[AES_CCM_16_64_128_IV_LEN]; uint16_t sequenceNumber; - // recipient context + // recipient context uint8_t recipientID[OSCOAP_MAX_ID_LEN]; uint8_t recipientIDLen; uint8_t recipientKey[AES_CCM_16_64_128_KEY_LEN]; uint8_t recipientIV[AES_CCM_16_64_128_IV_LEN]; - replay_window_t window; + replay_window_t window; } oscoap_security_context_t; typedef owerror_t (*callbackRx_cbt)(OpenQueueEntry_t* msg, @@ -190,7 +191,7 @@ struct coap_resource_desc_t { uint8_t path1len; uint8_t* path1val; uint8_t componentID; - oscoap_security_context_t* securityContext; + oscoap_security_context_t* securityContext; bool discoverable; callbackRx_cbt callbackRx; callbackSendDone_cbt callbackSendDone; @@ -198,7 +199,7 @@ struct coap_resource_desc_t { coap_resource_desc_t* next; }; -typedef struct { +typedef struct { uint8_t key[16]; uint8_t buffer[STATELESS_PROXY_STATE_LEN + STATELESS_PROXY_TAG_LEN]; uint8_t sequenceNumber; @@ -238,14 +239,14 @@ owerror_t opencoap_send( // option handling coap_option_class_t opencoap_get_option_class(coap_option_t type); uint8_t opencoap_options_encode(OpenQueueEntry_t* msg, - coap_option_iht* options, - uint8_t optionsLen, + coap_option_iht* options, + uint8_t optionsLen, coap_option_class_t class); uint8_t opencoap_options_parse(uint8_t* buffer, uint8_t bufferLen, coap_option_iht* options, uint8_t* optionsLen); -uint8_t opencoap_find_option(coap_option_iht* array, uint8_t arrayLen, coap_option_t option, uint8_t* startIndex); +uint8_t opencoap_find_option(coap_option_iht* array, uint8_t arrayLen, coap_option_t option, uint8_t* startIndex); /** \} diff --git a/openstack/02a-MAClow/IEEE802154E.c b/openstack/02a-MAClow/IEEE802154E.c index 4c2217c041..b94f04a4d5 100644 --- a/openstack/02a-MAClow/IEEE802154E.c +++ b/openstack/02a-MAClow/IEEE802154E.c @@ -883,6 +883,8 @@ port_INLINE void activity_ti1ORri1(void) { (errorparameter_t)ieee154e_vars.slotOffset, (errorparameter_t)0); + openserial_printBenchmark(BENCHMARK_EVENT_DESYNCHRONIZED, NULL, 0); + // update the statistics ieee154e_stats.numDeSync++; @@ -2699,6 +2701,7 @@ void changeIsSync(bool newIsSync) { if (ieee154e_vars.isSync==TRUE) { leds_sync_on(); resetStats(); + openserial_printBenchmark(BENCHMARK_EVENT_SYNCHRONIZED, NULL, 0); } else { leds_sync_off(); schedule_resetBackoff(); diff --git a/openstack/03a-IPHC/openbridge.c b/openstack/03a-IPHC/openbridge.c index 47c5691a03..e0928c077b 100644 --- a/openstack/03a-IPHC/openbridge.c +++ b/openstack/03a-IPHC/openbridge.c @@ -19,24 +19,24 @@ void openbridge_triggerData(void) { uint8_t input_buffer[136];//worst case: 8B of next hop + 128B of data OpenQueueEntry_t* pkt; uint8_t numDataBytes; - + numDataBytes = openserial_getInputBufferFillLevel(); - + //poipoi xv //this is a temporal workaround as we are never supposed to get chunks of data //longer than input buffer size.. I assume that HDLC will solve that. // MAC header is 13B + 8 next hop so we cannot accept packets that are longer than 118B if (numDataBytes>(136 - 10/*21*/) || numDataBytes<8){ - //to prevent too short or too long serial frames to kill the stack + //to prevent too short or too long serial frames to kill the stack openserial_printError(COMPONENT_OPENBRIDGE,ERR_INPUTBUFFER_LENGTH, (errorparameter_t)numDataBytes, (errorparameter_t)0); return; } - + //copying the buffer once we know it is not too big openserial_getInputBuffer(&(input_buffer[0]),numDataBytes); - + if (idmanager_getIsDAGroot()==TRUE && numDataBytes>0) { pkt = openqueue_getFreePacketBuffer(COMPONENT_OPENBRIDGE); if (pkt==NULL) { @@ -54,17 +54,22 @@ void openbridge_triggerData(void) { //payload packetfunctions_reserveHeaderSize(pkt,numDataBytes-8); memcpy(pkt->payload,&(input_buffer[8]),numDataBytes-8); - + //this is to catch the too short packet. remove it after fw-103 is solved. if (numDataBytes<16){ openserial_printError(COMPONENT_OPENBRIDGE,ERR_INVALIDSERIALFRAME, (errorparameter_t)0, (errorparameter_t)0); - } + } //send if ((iphc_sendFromBridge(pkt))==E_FAIL) { openqueue_freePacketBuffer(pkt); } + else { + // log the instant this packet has been sent by the dag root + // this is hack, we dump last 5 bytes to match possible benchmark packet that passes here + openserial_printBenchmark(BENCHMARK_EVENT_PACKETSENTDAGROOT, &pkt->payload[pkt->length-5], 5); + } } } @@ -75,25 +80,25 @@ void openbridge_sendDone(OpenQueueEntry_t* msg, owerror_t error) { (errorparameter_t)0, (errorparameter_t)0); } - openqueue_freePacketBuffer(msg); + openqueue_freePacketBuffer(msg); } /** \brief Receive a frame at the openbridge, which sends it out over serial. */ void openbridge_receive(OpenQueueEntry_t* msg) { - + // prepend previous hop packetfunctions_reserveHeaderSize(msg,LENGTH_ADDR64b); memcpy(msg->payload,msg->l2_nextORpreviousHop.addr_64b,LENGTH_ADDR64b); - + // prepend next hop (me) packetfunctions_reserveHeaderSize(msg,LENGTH_ADDR64b); memcpy(msg->payload,idmanager_getMyID(ADDR_64B)->addr_64b,LENGTH_ADDR64b); - + // send packet over serial (will be memcopied into serial buffer) openserial_printData((uint8_t*)(msg->payload),msg->length); - + // free packet openqueue_freePacketBuffer(msg); } diff --git a/openstack/03b-IPv6/forwarding.c b/openstack/03b-IPv6/forwarding.c index 7fcafc2588..50a4790152 100644 --- a/openstack/03b-IPv6/forwarding.c +++ b/openstack/03b-IPv6/forwarding.c @@ -303,6 +303,7 @@ void forwarding_receive( // populate packets metadata with L3 information memcpy(&(msg->l3_destinationAdd),&ipv6_inner_header->dest, sizeof(open_addr_t)); memcpy(&(msg->l3_sourceAdd), &ipv6_inner_header->src, sizeof(open_addr_t)); + msg->l3_hopLimit = ipv6_inner_header->hop_limit; if ( ( diff --git a/openstack/03b-IPv6/icmpv6rpl.h b/openstack/03b-IPv6/icmpv6rpl.h index f98f5d753b..8abcf7d28e 100644 --- a/openstack/03b-IPv6/icmpv6rpl.h +++ b/openstack/03b-IPv6/icmpv6rpl.h @@ -13,7 +13,7 @@ //=========================== define ========================================== #define DIO_PERIOD 10000 // in miliseconds -#define DAO_PERIOD 60000 // in miliseconds +#define DAO_PERIOD 30000 // in miliseconds // Non-Storing Mode of Operation (1) #define MOP_DIO_A 0<<5 diff --git a/openstack/SConscript b/openstack/SConscript index 110be02844..174442d94b 100644 --- a/openstack/SConscript +++ b/openstack/SConscript @@ -115,6 +115,7 @@ else: os.path.join('#','openapps','uinject'), os.path.join('#','openapps','userialbridge'), os.path.join('#','openapps','cjoin'), + os.path.join('#','openapps','cbenchmark'), os.path.join('#','openapps','uexpiration'), os.path.join('#','openapps','uexpiration_monitor'), ], diff --git a/openstack/cross-layers/idmanager.c b/openstack/cross-layers/idmanager.c index 660ba75f47..5a0288728d 100644 --- a/openstack/cross-layers/idmanager.c +++ b/openstack/cross-layers/idmanager.c @@ -273,10 +273,6 @@ void idmanager_getJoinKey(uint8_t **pKey) { return; } -void idmanager_setJoinAsn(asn_t* asn) { - memcpy(&idmanager_vars.joinAsn, asn, sizeof(asn_t)); -} - /** \brief Trigger this module to print status information, over serial. @@ -298,13 +294,4 @@ bool debugPrint_id(void) { return TRUE; } -bool debugPrint_joined(void) { - asn_t output; - output.byte4 = idmanager_vars.joinAsn.byte4; - output.bytes2and3 = idmanager_vars.joinAsn.bytes2and3; - output.bytes0and1 = idmanager_vars.joinAsn.bytes0and1; - openserial_printStatus(STATUS_JOINED,(uint8_t*)&output,sizeof(output)); - return TRUE; -} - //=========================== private ========================================= diff --git a/openstack/cross-layers/idmanager.h b/openstack/cross-layers/idmanager.h index 89c4063220..04ab511a1f 100644 --- a/openstack/cross-layers/idmanager.h +++ b/openstack/cross-layers/idmanager.h @@ -44,7 +44,6 @@ typedef struct { open_addr_t myPrefix; bool slotSkip; uint8_t joinKey[16]; - asn_t joinAsn; } idmanager_vars_t; //=========================== prototypes ====================================== @@ -58,11 +57,9 @@ owerror_t idmanager_setMyID(open_addr_t* newID); bool idmanager_isMyAddress(open_addr_t* addr); void idmanager_triggerAboutRoot(void); void idmanager_setJoinKey(uint8_t *key); -void idmanager_setJoinAsn(asn_t *asn); void idmanager_getJoinKey(uint8_t **pKey); bool debugPrint_id(void); -bool debugPrint_joined(void); /** \} \} diff --git a/projects/python/SConscript.env b/projects/python/SConscript.env index 6c65e2d844..90c9a31d25 100644 --- a/projects/python/SConscript.env +++ b/projects/python/SConscript.env @@ -91,6 +91,7 @@ buildEnv.Append( os.path.join('#','build','python_gcc','openapps','uinject'), os.path.join('#','build','python_gcc','openapps','userialbridge'), os.path.join('#','build','python_gcc','openapps','cjoin'), + os.path.join('#','build','python_gcc','openapps','cbenchmark'), ] ) @@ -159,6 +160,7 @@ varsToChange = [ 'cexample_vars', 'cinfo_vars', 'cjoin_vars', + 'cbenchmark_vars', 'cleds_vars', 'csensors_vars', 'cstorm_vars', @@ -216,6 +218,7 @@ callbackFunctionsToChange = [ 'rxCb', #===== drivers # openserial + 'callbackSendPacket', # opentimers 'callback', #===== kernel @@ -348,6 +351,7 @@ functionsToChange = [ 'radio_setTimerPeriod', 'radio_getTimerPeriod', 'radio_setFrequency', + 'radio_setTxPower', 'radio_rfOn', 'radio_rfOff', 'radio_loadPacket', @@ -405,7 +409,9 @@ functionsToChange = [ # openserial 'openserial_init', 'openserial_register', + 'openserial_registerSendPacketCb', 'openserial_printStatus', + 'openserial_printBenchmark', 'openserial_printInfoErrorCritical', 'openserial_printData', 'openserial_printSniffedPacket', @@ -822,7 +828,6 @@ functionsToChange = [ 'idmanager_triggerAboutRoot', 'idmanager_triggerAboutBridge', 'idmanager_setJoinKey', - 'idmanager_setJoinAsn', 'idmanager_getJoinKey', 'debugPrint_id', 'debugPrint_joined', @@ -974,6 +979,14 @@ functionsToChange = [ 'cjoin_retransmission_task_cb', 'cjoin_getIsJoined', 'cjoin_setIsJoined', + # cbenchmark + 'cbenchmark_init', + 'cbenchmark_receive', + 'cbenchmark_sendDone', + 'cbenchmark_sendPacket', + 'cbenchmark_parse_sendPacket', + 'cbenchmark_printSerial_packetSentReceived', + 'cbenchmark_sendPacket_task_cb', ] headerFiles = [ @@ -1037,6 +1050,7 @@ headerFiles = [ 'cleds', 'cstorm', 'cwellknown', + 'cbenchmark', #'techo', 'tohlone', 'uecho', @@ -1044,6 +1058,7 @@ headerFiles = [ 'userialbridge', 'rrt', 'cjoin', + 'cbenchmark', 'uexpiration', 'uexpiration_monitor' ]