Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • arp
  • arp-gyro
  • async-fsm
  • cc3135
  • chipselect-mux
  • logger-documentation
  • main
  • mockup-main-software
  • nas-catch-dev
  • nas-pitot-correction
  • nd015x-dma
  • nokia-tm-dev
  • parafoil-mavlink-upd
  • quadspi-flash
  • quadspi-flash2
  • spi
  • sx1278-resilience
  • units-impl
  • ARP-pre-2.7
  • PYXIS_EUROC
  • PYXIS_ROCCARASO
  • hermes-flight-1
  • hermes-v1.0
  • lynx-euroc
24 results

Target

Select target project
  • avn/swd/skyward-boardcore
  • emilio.corigliano/skyward-boardcore
  • ettore.pane/skyward-boardcore
  • giulia.facchi/skyward-boardcore
  • valerio.flamminii/skyward-boardcore
  • nicolo.caruso/skyward-boardcore
6 results
Select Git revision
  • ASM330LHH-dev
  • LSM9DS1-test
  • Lis3mdl-bugfix
  • ads131m08
  • aspis-dev
  • build-system
  • cc3135
  • clang
  • clang-support-dev
  • f769-disco
  • hil-upd
  • kalman-fix
  • lps22df-dev
  • lps28dfw
  • lsm6dsrx
  • main
  • neo-m6
  • parafoil
  • parafoil-f103-SDlog
  • parafoil-sensortile
  • rd-gs-nas-extractor
  • stepper-logging-upd
  • sx1278
  • tfmicro-dev
  • timed-wait
  • timed-wait2
  • ubxgps-serial-upd
  • vcm
  • vl53l7cxa-dev
  • vn100-upd
  • vn300
  • PYXIS_EUROC
  • PYXIS_ROCCARASO
  • hermes-flight-1
  • hermes-v1.0
  • lynx-euroc
36 results
Show changes
Showing
with 0 additions and 4620 deletions
/* Copyright (c) 2018-2019 Skyward Experimental Rocketry
* Author: Luca Erbetta
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <miosix.h>
#include <sensors/Sensor.h>
#include <stdint.h>
#include <utils/Debug.h>
#include "AD7994Data.h"
/**
* Driver for the AD7994 Analog Digital Converter
* CLASS INVARIANT: The address pointer register points to the conversion result
* register.
* This is done for performance reasons: we don't want to write to the
* address pointer register each time we perform a conversion (eg multiple times
* per second)
*/
template <typename BusI2C, typename BusyPin, typename CONVST>
class AD7994 : public Sensor
{
public:
enum class Channel : uint8_t
{
CH1 = 1,
CH2,
CH3,
CH4
};
/**
* @brief AD7994 constructor. Invariant is respected as the sensor powers up
* pointing at the conversion result register.
*
* @param i2c_address I2C address of the AD7994
*/
AD7994(uint8_t i2c_address)
: i2c_address(i2c_address), enabled_channels{0, 0, 0, 0}
{
}
~AD7994() {}
/**
* @brief Initialize the sensor writing the configuration register
* This driver is designed to work in Mode 1
* The configuration register is set to enable the BUSY pin, I2C filtering
* and no enabled channels.
* @return true If the sensor was configured successfully
*/
bool init() override
{
uint8_t config_reg_value = 0x0A; // 0b00001010
// Write the configuration register
BusI2C::write(i2c_address, REG_CONFIG, &config_reg_value, 1);
uint8_t read_config_reg_value;
// Read back the value
BusI2C::directRead(i2c_address, &read_config_reg_value, 1);
// BusI2C::read(i2c_address, REG_CONFIG, &read_config_reg_value, 1);
pointToConversionResult();
return read_config_reg_value == config_reg_value;
}
void enableChannel(Channel channel)
{
uint8_t ch = static_cast<uint8_t>(channel);
enabled_channels[ch - 1] = true;
uint8_t config_reg_value;
BusI2C::read(i2c_address, REG_CONFIG, &config_reg_value, 1);
// Update the config register value
uint8_t channel_reg = 0;
for (int i = 0; i < 4; i++)
{
if (enabled_channels[i])
channel_reg |= 1 << i;
}
config_reg_value = (config_reg_value & 0x0F) | channel_reg << 4;
BusI2C::write(i2c_address, REG_CONFIG, &config_reg_value, 1);
pointToConversionResult();
}
void disableChannel(Channel channel)
{
uint8_t ch = static_cast<uint8_t>(channel);
enabled_channels[ch - 1] = false;
uint8_t channel_reg = 0;
for (int i = 0; i < 4; i++)
{
if (enabled_channels[i])
channel_reg |= 1 << i;
}
uint8_t config_reg_value;
BusI2C::read(i2c_address, REG_CONFIG, &config_reg_value, 1);
// Update the config register value
config_reg_value = (config_reg_value & 0x0F) | channel_reg << 4;
BusI2C::write(i2c_address, REG_CONFIG, &config_reg_value, 1);
pointToConversionResult();
}
/**
* @brief Triggers a new ADC conversion on the enabled channels and reads
* the value of the conversion register.
* TODO: Check how to sample multiple channels.
* @return true If the conversion was successful on all enabled channels
*/
bool onSimpleUpdate() override
{
uint8_t data[2];
for (int i = 0; i < 4; i++)
{
if (enabled_channels[i])
{
// Trigger a conversion
CONVST::high();
miosix::delayUs(3);
CONVST::low();
// Wait for the conversion to complete
miosix::delayUs(2);
BusI2C::directRead(i2c_address, data, 2);
samples[i] = decodeConversion(data);
samples[i].timestamp = miosix::getTick();
}
}
return true;
}
bool selfTest() { return true; }
AD7994Sample getLastSample(Channel channel)
{
uint8_t ch = static_cast<uint8_t>(channel);
return samples[ch - 1];
}
private:
// Address of the AD7994 on the I2C bus
const uint8_t i2c_address;
// The 4 LSBs indicate where the corresponding channel is enabled or not.
bool enabled_channels[4];
/**
* @brief Writes the conversion register address to the address pointer
* register in order to restore the class invariant.
*/
void pointToConversionResult()
{
uint8_t reg_addr = REG_CONVERSION_RESULT;
BusI2C::directWrite(i2c_address, &reg_addr, 1);
}
/**
* @brief Decodes the 2 bytes of the conversion register into an
* AD7994Sample structure
*
* @param data_ptr Pointer to the first of the 2 bytes of the conversione
* register
* @return AD7994Sample
*/
AD7994Sample decodeConversion(uint8_t* data_ptr)
{
uint16_t conv_reg =
static_cast<uint16_t>((data_ptr[0]) << 8) + data_ptr[1];
AD7994Sample out;
out.value = conv_reg & 0x0FFF;
out.channel_id = (static_cast<uint8_t>(conv_reg >> 12) & 0x03) + 1;
out.alert_flag = static_cast<bool>(conv_reg >> 15);
return out;
}
enum Registers : uint8_t
{
REG_CONVERSION_RESULT = 0x00,
REG_ALERT_STATUS = 0x01,
REG_CONFIG = 0x02,
};
AD7994Sample samples[4];
};
/* Copyright (c) 2018 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "IsbProtocol_serial2.h"
using namespace std;
using namespace miosix;
typedef Gpio<GPIOA_BASE, 2> u2tx;
typedef Gpio<GPIOA_BASE, 3> u2rx;
typedef Gpio<GPIOA_BASE, 1> u2rts;
void __attribute__((weak)) USART2_IRQHandler()
{
saveContext();
asm volatile("bl _Z11Serial2_Irqv");
restoreContext();
}
void __attribute__((used)) Serial2_Irq()
{
auto inst = IsbProtocol_serial2::instance();
inst.IRQHandler();
}
IsbProtocol_serial2::IsbProtocol_serial2()
{
{
FastInterruptDisableLock dLock;
RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
RCC_SYNC();
u2tx::mode(Mode::ALTERNATE);
u2tx::alternateFunction(7);
#ifdef _ARCH_CORTEXM3_STM32F1
u2rx::mode(Mode::INPUT);
#else
u2rx::mode(Mode::ALTERNATE);
u2rx::alternateFunction(7);
#endif
u2rts::mode(Mode::OUTPUT);
u2rts::low();
}
/*** USART 2 configuration ***/
// Enable parity, 9 bit frame length, no parity,
// generate interrupt in case a new byte is received
USART2->CR1 |= USART_CR1_M | USART_CR1_RXNEIE | USART_CR1_TE // enable tx
| USART_CR1_RE; // enable rx
// CR2 and CR3 registers are left untouched since their default values are
// OK
NVIC_SetPriority(USART2_IRQn, 15); // Lowest priority for serial
NVIC_ClearPendingIRQ(USART2_IRQn);
NVIC_EnableIRQ(USART2_IRQn);
}
IsbProtocol_serial2::~IsbProtocol_serial2()
{
FastInterruptDisableLock dLock;
RCC->APB1ENR &= ~RCC_APB1ENR_USART2EN;
RCC_SYNC();
}
void IsbProtocol_serial2::IRQHandler()
{
if (USART2->SR & USART_SR_RXNE)
{
/* This protocol uses 9 bit long usart frames.
* The STM32's data register is 9 bit long, so the
* lower 8 bits are used to transport the data byte and
* the MSB - the 9th bit - is used to select between
* data or address byte
*/
uint16_t byte = USART->DR;
uint16_t checkMask = 0x100 | nodeAddress;
if (byte == checkMask)
{
rxStatus.rxIndex = 0;
rxStatus.rxInProgress = true;
rxStatus.dataLenPending = true;
rxBuf[rxStatus.rxIndex] = byte & 0xFF; // strip away the 9th byte
rxStatus.rxIndex++;
}
else if (rxStatus.rxInProgress)
{
if (rxStatus.dataLenPending)
{
// Second byte in the packet is the data len field. To this
// value we have to add 4 bytes: address, data len and two bytes
// of CRC
rxStatus.packetSize = (byte & 0xFF) + 4;
rxStatus.dataLenPending = false;
}
rxBuf[rxStatus.rxIndex] = byte & 0xFF;
rxStatus.rxIndex++;
if (rxStatus.rxIndex >= rxStatus.packetSize)
{
rxStatus.rxInProgress = false;
}
}
}
}
size_t IsbProtocol_serial2::newDataAvailable()
{
if ((rxStatus.rxInProgress == true) || (rxStatus.packetSize = 0))
{
return 0;
}
// we pass packetSize - 2 in order to avoid including in the CRC calculation
// the CRC value inserted from the sender
uint16_t crc = CRC16(rxBuf, rxStatus.packetSize - 2);
uint16_t pktCrc =
(rxBuf[rxStatus.packetSize - 2] << 8) | rxBuf[rxStatus.packetSize - 1];
// Packet length check: the value contained in the second byte must be
// equal to the number of bytes received minus 4
bool checkLen = (rxBuf[1] == (rxStatus.packetSize - 4)) ? true : false;
if ((crc != pktCrc) || (!checkLen))
{
rxStatus.packetSize = 0; // packet is corrupt, discard it
return 0;
}
// Return length of the data field
return rxBuf[1];
}
void IsbProtocol_serial2::getData(uint8_t* buffer)
{
std::memcpy(buffer, rxBuf[2], rxBuf[1]);
}
void IsbProtocol_serial2::sendData(uint8_t dstAddr, uint8_t* data, size_t len)
{
// packet too long
if (len > 0xFF)
{
return;
}
txBuf[0] = dstAddr;
txBuf[1] = len;
std::memcpy(txBuf[2], data, len);
// Include in CRC also address and data len field
uint16_t crc = CRC16(txBuf, len + 2);
size_t crcBegin = len + 2;
txBuf[crcBegin] = crc >> 8;
txBuf[crcBegin + 1] = crc & 0xFF;
u2rts::high();
for (size_t i = 0; i < len + 4; i++)
{
// the first byte is the address, so it has to be sent with the 9th bit
// set to 1
if (i == 0)
{
USART2->DR = txBuf[i] | 0x100;
}
else
{
USART2->DR = txBuf[i];
}
// Wait until tx buffer is empty again
while ((USART2->SR & USART_SR_TXE) == 0)
;
}
u2rts::low();
}
void IsbProtocol_serial2::setBaud(uint32_t baud)
{
uint32_t busFreq = SystemCoreClock;
#ifdef _ARCH_CORTEXM3_STM32F1
if (RCC->CFGR & RCC_CFGR_PPRE1_2)
{
busFreq /= 1 << (((RCC->CFGR >> 8) & 0x3) + 1);
}
#else
if (RCC->CFGR & RCC_CFGR_PPRE1_2)
{
busFreq /= 1 << (((RCC->CFGR >> 10) & 0x3) + 1);
}
#endif
uint32_t quot = 2 * busFreq / baud; // 2*freq for round to nearest
USART2->BRR = quot / 2 + (quot & 1);
}
void IsbProtocol_serial2::setNodeAddress(uint8_t address)
{
nodeAddress = address;
}
uint16_t IsbProtocol_serial2::CRC16(uint8_t* data, size_t len)
{
uint16_t crc = 0xFFFF;
for (size_t i = 0; i < len; i++)
{
uint16_t x = ((crc >> 8) ^ data[i]) & 0xff;
x ^= x >> 4;
crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x;
}
return crc;
}
IsbProtocol_serial2& IsbProtocol_serial2::instance()
{
static IsbProtocol_serial2 inst;
return inst;
}
/* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <stddef.h>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include "miosix.h"
/* This class is a kind of driver to be used to excange data between the
* stormtrooper master and the other stormtrooper slave boards through the
* RS485 lines. The communication model is this: all the boards are connected
* on the same bus and each board has a unique node address. To avoid conflicts,
* the master is the only device on the bus that can initiate a communication;
* thus, to communicate with a specific slave, the master sends a packet
* containing the targeted slave address and the data and then waits for the
* response (if expected). Owing to this model, if two stormtrooper slaves need
* to exchange data they have to rely on the stormtrooper master acting as a
* router between them.
*
* The packet structure is this:
*
* +------+-----+------+-----+
* | addr | len | data | CRC |
* +------+-----+------+-----+
*
* -> addr: target node address, 8 bit
* -> len: lenght of the data field, 8 bit
* -> data: data bytes, up to 255
* -> CRC: 16 bit field calculated on addr, len and data using the ccitt CRC16
* formula
*
* On the bus data is sent using 9 bit long frames with one stop bit. If the 9th
* bit is set to 1 means that the byte received is an address byte and so a
* packet is incoming
*
*
* Here is a sample code about this class' usage:
*
* auto comm = IsbProtocol_serial2::instance();
*
* comm.setBaud(9600); //set baud to 9600 bps
* comm.setNodeAddress(0xAB); //set node ID
*
* // this to check if new data arrived and to copy the bytes into a local
* buffer
*
* uint8_t rx_buf[256];
*
* if(comm.newDataAvailable() > 0)
* {
* comm.getData(rx_buf);
* }
*
* // this to send data
*
* uint8_t tx_buf[] = "Some data for you! :)";
*
* comm.sendData(0xE0, tx_buf, sizeof(tx_buf));
*
*/
class IsbProtocol_serial2
{
public:
/**
* @return singleton intance of the class
*/
static IsbProtocol_serial2& instance();
~IsbProtocol_serial2();
/**
* Set the USART baud rate.
* @param baud baud rate to be set
*/
void setBaud(uint32_t baud);
/**
* Set the node's address in the network
*/
void setNodeAddress(uint8_t address);
/**
* @return number of payload data bytes received or 0 if none received
*/
size_t newDataAvailable();
/**
* Get the latest payload data received.
* @param buffer pointer to buffer in which copy the new data, it must have
* dimension greater than or equal to the value returned by
* newDataAvailable()
*/
void getData(uint8_t* buffer);
/**
* Send a packet, this function blocks until all data is sent
*
* @param dstAddr destination node address
* @param data pointer to a buffer containing the payload data
* @param len payload size in bytes
*/
void sendData(uint8_t dstAddr, uint8_t* data, size_t len);
/**
* Interrupt handler. DON'T CALL IT ANYWHERE!!
*/
void IRQHandler();
private:
uint8_t rxBuf[300];
struct R
{
uint16_t packetSize;
uint16_t rxIndex;
bool rxInProgress;
bool dataLenPending;
} rxStatus;
uint8_t txBuf[300];
uint8_t nodeAddress;
uint16_t CRC16(uint8_t* data, size_t len);
IsbProtocol_serial2();
IsbProtocol_serial2(const IsbProtocol_serial2& other);
IsbProtocol_serial2& operator=(const IsbProtocol_serial2& other);
bool operator==(const IsbProtocol_serial2& other);
};
/*
* Inter Stormtrooper Boards communication protocol through STM32's
* USART3 interface
*
* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
/* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "IsbProtocol_serial3.h"
using namespace std;
using namespace miosix;
typedef Gpio<GPIOB_BASE, 10> u3tx;
typedef Gpio<GPIOB_BASE, 11> u3rx;
typedef Gpio<GPIOB_BASE, 14> u3rts;
void __attribute__((weak)) USART3_IRQHandler()
{
saveContext();
asm volatile("bl _Z11Serial3_Irqv");
restoreContext();
}
void __attribute__((used)) Serial3_Irq()
{
auto inst = IsbProtocol_serial3::instance();
inst.IRQHandler();
}
IsbProtocol_serial3::IsbProtocol_serial3()
{
{
FastInterruptDisableLock dLock;
RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
RCC_SYNC();
u3tx::mode(Mode::ALTERNATE);
u3tx::alternateFunction(7);
#ifdef _ARCH_CORTEXM3_STM32F1
u3rx::mode(Mode::INPUT);
#else
u3rx::mode(Mode::ALTERNATE);
u3rx::alternateFunction(7);
#endif
u3rts::mode(Mode::OUTPUT);
u3rts::low();
}
/*** USART 3 configuration ***/
// Enable parity, 9 bit frame length, no parity,
// generate interrupt in case a new byte is received
USART3->CR1 |= USART_CR1_M | USART_CR1_RXNEIE | USART_CR1_TE // enable tx
| USART_CR1_RE; // enable rx
// CR2 and CR3 registers are left untouched since their default values are
// OK
NVIC_SetPriority(USART3_IRQn, 15); // Lowest priority for serial
NVIC_ClearPendingIRQ(USART3_IRQn);
NVIC_EnableIRQ(USART3_IRQn);
}
IsbProtocol_serial3::~IsbProtocol_serial3()
{
FastInterruptDisableLock dLock;
RCC->APB1ENR &= ~RCC_APB1ENR_USART3EN;
RCC_SYNC();
}
void IsbProtocol_serial3::IRQHandler()
{
if (USART3->SR & USART_SR_RXNE)
{
/* This protocol uses 9 bit long usart frames.
* The STM32's data register is 9 bit long, so the
* lower 8 bits are used to transport the data byte and
* the MSB - the 9th bit - is used to select between
* data or address byte
*/
uint16_t byte = USART->DR;
uint16_t checkMask = 0x100 | nodeAddress;
if (byte == checkMask)
{
rxStatus.rxIndex = 0;
rxStatus.rxInProgress = true;
rxStatus.dataLenPending = true;
rxBuf[rxStatus.rxIndex] = byte & 0xFF; // strip away the 9th byte
rxStatus.rxIndex++;
}
else if (rxStatus.rxInProgress)
{
if (rxStatus.dataLenPending)
{
// Second byte in the packet is the data len field. To this
// value we have to add 4 bytes: address, data len and two bytes
// of CRC
rxStatus.packetSize = (byte & 0xFF) + 4;
rxStatus.dataLenPending = false;
}
rxBuf[rxStatus.rxIndex] = byte & 0xFF;
rxStatus.rxIndex++;
if (rxStatus.rxIndex >= rxStatus.packetSize)
{
rxStatus.rxInProgress = false;
}
}
}
}
size_t IsbProtocol_serial3::newDataAvailable()
{
if ((rxStatus.rxInProgress == true) || (rxStatus.packetSize = 0))
{
return 0;
}
// we pass packetSize - 2 in order to avoid including in the CRC calculation
// the CRC value inserted from the sender
uint16_t crc = CRC16(rxBuf, rxStatus.packetSize - 2);
uint16_t pktCrc =
(rxBuf[rxStatus.packetSize - 2] << 8) | rxBuf[rxStatus.packetSize - 1];
// Packet length check: the value contained in the second byte must be
// equal to the number of bytes received minus 4
bool checkLen = (rxBuf[1] == (rxStatus.packetSize - 4)) ? true : false;
if ((crc != pktCrc) || (!checkLen))
{
rxStatus.packetSize = 0; // packet is corrupt, discard it
return 0;
}
// Return length of the data field
return rxBuf[1];
}
void IsbProtocol_serial3::getData(uint8_t* buffer)
{
std::memcpy(buffer, rxBuf[2], rxBuf[1]);
}
void IsbProtocol_serial3::sendData(uint8_t dstAddr, uint8_t* data, size_t len)
{
// packet too long
if (len > 0xFF)
{
return;
}
txBuf[0] = dstAddr;
txBuf[1] = len;
std::memcpy(txBuf[2], data, len);
// Include in CRC also address and data len field
uint16_t crc = CRC16(txBuf, len + 2);
size_t crcBegin = len + 2;
txBuf[crcBegin] = crc >> 8;
txBuf[crcBegin + 1] = crc & 0xFF;
u3rts::high();
for (size_t i = 0; i < len + 4; i++)
{
// the first byte is the address, so it has to be sent with the 9th bit
// set to 1
if (i == 0)
{
USART3->DR = txBuf[i] | 0x100;
}
else
{
USART3->DR = txBuf[i];
}
// Wait until tx buffer is empty again
while ((USART3->SR & USART_SR_TXE) == 0)
;
}
u3rts::low();
}
void IsbProtocol_serial3::setBaud(uint32_t baud)
{
uint32_t busFreq = SystemCoreClock;
#ifdef _ARCH_CORTEXM3_STM32F1
if (RCC->CFGR & RCC_CFGR_PPRE1_2)
{
busFreq /= 1 << (((RCC->CFGR >> 8) & 0x3) + 1);
}
#else
if (RCC->CFGR & RCC_CFGR_PPRE1_2)
{
busFreq /= 1 << (((RCC->CFGR >> 10) & 0x3) + 1);
}
#endif
uint32_t quot = 2 * busFreq / baud; // 2*freq for round to nearest
USART3->BRR = quot / 2 + (quot & 1);
}
void IsbProtocol_serial3::setNodeAddress(uint8_t address)
{
nodeAddress = address;
}
uint16_t IsbProtocol_serial3::CRC16(uint8_t* data, size_t len)
{
uint16_t crc = 0xFFFF;
for (size_t i = 0; i < len; i++)
{
uint16_t x = ((crc >> 8) ^ data[i]) & 0xff;
x ^= x >> 4;
crc = (crc << 8) ^ (x << 12) ^ (x << 5) ^ x;
}
return crc;
}
IsbProtocol_serial3& IsbProtocol_serial3::instance()
{
static IsbProtocol_serial3 inst;
return inst;
}
/* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <stddef.h>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include "miosix.h"
/* This class is a kind of driver to be used to excange data between the
* stormtrooper master and the other stormtrooper slave boards through the
* RS485 lines. The communication model is this: all the boards are connected
* on the same bus and each board has a unique node address. To avoid conflicts,
* the master is the only device on the bus that can initiate a communication;
* thus, to communicate with a specific slave, the master sends a packet
* containing the targeted slave address and the data and then waits for the
* response (if expected). Owing to this model, if two stormtrooper slaves need
* to exchange data they have to rely on the stormtrooper master acting as a
* router between them.
*
* The packet structure is this:
*
* +------+-----+------+-----+
* | addr | len | data | CRC |
* +------+-----+------+-----+
*
* -> addr: target node address, 8 bit
* -> len: lenght of the data field, 8 bit
* -> data: data bytes, up to 255
* -> CRC: 16 bit field calculated on addr, len and data using the ccitt CRC16
* formula
*
* On the bus data is sent using 9 bit long frames with one stop bit. If the 9th
* bit is set to 1 means that the byte received is an address byte and so a
* packet is incoming
*
*
* Here is a sample code about this class' usage:
*
* auto comm = IsbProtocol_serial2::instance();
*
* comm.setBaud(9600); //set baud to 9600 bps
* comm.setNodeAddress(0xAB); //set node ID
*
* // this to check if new data arrived and to copy the bytes into a local
* buffer
*
* uint8_t rx_buf[256];
*
* if(comm.newDataAvailable() > 0)
* {
* comm.getData(rx_buf);
* }
*
* // this to send data
*
* uint8_t tx_buf[] = "Some data for you! :)";
*
* comm.sendData(0xE0, tx_buf, sizeof(tx_buf));
*
*/
class IsbProtocol_serial3
{
public:
/**
* @return singleton intance of the class
*/
static IsbProtocol_serial3& instance();
~IsbProtocol_serial3();
/**
* Set the USART baud rate.
* @param baud baud rate to be set
*/
void setBaud(uint32_t baud);
/**
* Set the node's address in the network
*/
void setNodeAddress(uint8_t address);
/**
* @return number of payload data bytes received or 0 if none received
*/
size_t newDataAvailable();
/**
* Get the latest payload data received.
* @param buffer pointer to buffer in which copy the new data, it must have
* dimension greater than or equal to the value returned by
* newDataAvailable()
*/
void getData(uint8_t* buffer);
/**
* Send a packet, this function blocks until all data is sent
*
* @param dstAddr destination node address
* @param data pointer to a buffer containing the payload data
* @param len payload size in bytes
*/
void sendData(uint8_t dstAddr, uint8_t* data, size_t len);
/**
* Interrupt handler. DON'T CALL IT ANYWHERE!!
*/
void IRQHandler();
private:
uint8_t rxBuf[300];
struct R
{
uint16_t packetSize;
uint16_t rxIndex;
bool rxInProgress;
bool dataLenPending;
} rxStatus;
uint8_t txBuf[300];
uint8_t nodeAddress;
uint16_t CRC16(uint8_t* data, size_t len);
IsbProtocol_serial3();
IsbProtocol_serial3(const IsbProtocol_serial3& other);
IsbProtocol_serial3& operator=(const IsbProtocol_serial3& other);
bool operator==(const IsbProtocol_serial3& other);
};
/* Copyright (c) 2016-2017 Skyward Experimental Rocketry
* Authors: Alain Carlucci, Federico Terraneo
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "Leds.h"
// Relying on the BSP to provide leds and configure them as output
#define LED(x) miosix::leds::led##x::getPin()
std::array<miosix::GpioPin, Leds::numLeds> Leds::pins{
{LED(0), LED(1), LED(2), LED(3), LED(4), LED(5), LED(6), LED(7), LED(8),
LED(9)}};
#undef LED
/* Copyright (c) 2016-2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "PacketBuffer.h"
#include <utils/Debug.h>
using namespace std;
using namespace miosix;
PacketBuffer::PacketBuffer(size_t storageSize)
: storageSize(storageSize), usedSize(0), writeIndex(0), readIndex(0),
valid(true)
{
#ifndef __NO_EXCEPTIONS
try
{
buffer = new uint8_t[storageSize];
}
catch (std::bad_alloc& exc)
{
TRACE("bad alloc!\n");
valid = false;
}
#else
buffer = new (std::nothrow) uint8_t[storageSize];
if (buffer == nullptr)
{
valid = false;
}
#endif
}
PacketBuffer::~PacketBuffer()
{
if (valid)
delete[] buffer;
}
bool PacketBuffer::push(packet_header_t& header, const uint8_t* payload)
{
if (!valid)
return false;
size_t packetSize = sizeof(packet_header_t) + header.payloadSize;
// Simple technique to avoid ring buffer writeIndex slip ahead readIndex:
// we keep track of the amount of space used and we reject a new incoming
// packet if usedSize + packetSize is greater that the ring buffer size.
if (usedSize + packetSize >= storageSize)
return false;
// to store the packet into the ring buffer first copy the header
// and then copy the payload
{
Lock<FastMutex> l(mutex);
uint8_t* head = reinterpret_cast<uint8_t*>(&header);
for (unsigned int i = 0; i < sizeof(packet_header_t); i++)
{
buffer[(writeIndex + i) % storageSize] = head[i];
}
writeIndex = (writeIndex + sizeof(packet_header_t)) % storageSize;
for (unsigned int i = 0; i < header.payloadSize; i++)
{
buffer[(writeIndex + i) % storageSize] = payload[i];
}
writeIndex = (writeIndex + header.payloadSize) % storageSize;
usedSize += packetSize;
}
return true;
}
packet_header_t PacketBuffer::getHeader()
{
packet_header_t header = {};
uint8_t* dest = reinterpret_cast<uint8_t*>(&header);
memset(dest, 0x00, sizeof(packet_header_t));
if (valid)
{
Lock<FastMutex> l(mutex);
for (unsigned int i = 0; i < sizeof(packet_header_t); i++)
dest[i] = buffer[(readIndex + i) % storageSize];
}
return header;
}
void PacketBuffer::getData(uint8_t* data)
{
if (!valid)
return;
/* Packet size is stored in header, so first get the payloadSize.
* The data is stored sizeof(packet_header_t) bytes next the read pointer,
* since a packet is stored as header followed by payload, so we have to
* add sizeof(packet_header_t) to the read pointer to reach the first data
* byte
*/
packet_header_t frontHead = getHeader();
{
Lock<FastMutex> l(mutex);
int start = readIndex + sizeof(packet_header_t);
for (unsigned int i = 0; i < frontHead.payloadSize; i++)
{
data[i] = buffer[(start + i) % storageSize];
}
}
}
void PacketBuffer::popFront()
{
if ((!valid) || empty()) // this means that list is empty
return;
/* Popping a packet out of list simply means move the readIndex forward
* of the size of the packet at list's head.
* The size of a packet is made of two parts: a fixed one which is
* constituded by the size of IP address, port and packet length fields
* and a variable one that is the size of the payload. The latter is stored
* into the len field of the packet.
* What we do here is gather the fixed and variable sizes and sum them
* together to obtain the readIndex's needed increment
*/
packet_header_t head = getHeader();
size_t packetSize = sizeof(packet_header_t) + head.payloadSize;
{
Lock<FastMutex> l(mutex);
readIndex = (readIndex + packetSize) % storageSize;
usedSize -= packetSize;
}
}
bool PacketBuffer::empty()
{
Lock<FastMutex> l(mutex);
return usedSize == 0;
}
/* Copyright (c) 2016-2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "UdpManager.h"
using namespace std;
using namespace miosix;
void __attribute__((naked)) EXTI1_IRQHandler()
{
saveContext();
asm volatile("bl _Z13EXTIrqHandlerv");
restoreContext();
}
void __attribute__((used)) EXTIrqHandler()
{
EXTI->PR |= EXTI_PR_PR1;
Singleton<UdpManager>::getInstance().phyIrqHandler();
}
void _evt_mgmt_thread(void* args)
{
Singleton<UdpManager>::getInstance().evtQueue.run();
}
UdpManager::UdpManager()
{
eth::int1::mode(Mode::INPUT);
// Configure STM32 to generate an interrupt on
// falling edge of chip's INT line
{
FastInterruptDisableLock dLock;
RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
RCC_SYNC();
}
SYSCFG->EXTICR[0] |= SYSCFG_EXTICR1_EXTI1_PC;
EXTI->IMR |= EXTI_IMR_MR1;
EXTI->FTSR |= EXTI_FTSR_TR1;
NVIC_SetPriority(EXTI1_IRQn, 10);
NVIC_ClearPendingIRQ(EXTI1_IRQn);
NVIC_EnableIRQ(EXTI1_IRQn);
phy.setModeReg(0x00);
phy.setSocketInterruptMask(0xFF);
txBuffer = new PacketBuffer(TX_BUF_SIZE);
rxBuffer = new PacketBuffer(RX_BUF_SIZE);
wdt->setDuration(TX_TIMEOUT);
wdt->setCallback(bind(&UdpManager::wdtIrqHandler, this));
if (!txBuffer->isValid() || !rxBuffer->isValid())
{
// TODO log!
}
else
{
Thread::create(_evt_mgmt_thread, 1024);
}
}
UdpManager::~UdpManager()
{
delete txBuffer;
delete rxBuffer;
}
void UdpManager::setTxPort(uint16_t port)
{
// Open transmitting socket
phy.setSocketModeReg(PHY_TX_SOCK_NUM, SOCKn_MR_UDP);
phy.setSocketSourcePort(PHY_TX_SOCK_NUM, port);
// Enable timeout & send ok interrupts
phy.setSocketInterruptMaskReg(PHY_TX_SOCK_NUM, 0x18);
phy.setSocketCommandReg(PHY_TX_SOCK_NUM, SOCKn_CR_OPEN);
}
void UdpManager::setRxPort(uint16_t port)
{
// Open receiving socket
phy.setSocketModeReg(PHY_RX_SOCK_NUM, SOCKn_MR_UDP);
phy.setSocketSourcePort(PHY_RX_SOCK_NUM, port);
// Enable recv interrupt only
phy.setSocketInterruptMaskReg(PHY_RX_SOCK_NUM, 0x04);
phy.setSocketCommandReg(PHY_RX_SOCK_NUM, SOCKn_CR_OPEN);
phy.setSocketCommandReg(PHY_RX_SOCK_NUM, SOCKn_CR_RECV);
}
/** Legacy code, uncomment if we want tx @ port and rx @ port+1 **/
// void UdpManager::setPort(uint16_t port) {
//
// //Open transmitting socket
// phy.setSocketModeReg(PHY_TX_SOCK_NUM,SOCKn_MR_UDP);
// phy.setSocketSourcePort(PHY_TX_SOCK_NUM,port);
// // Enable timeout & send ok interrupts
// phy.setSocketInterruptMaskReg(PHY_TX_SOCK_NUM,0x18);
// phy.setSocketCommandReg(PHY_TX_SOCK_NUM,SOCKn_CR_OPEN);
//
// //Open receiving socket
// phy.setSocketModeReg(PHY_RX_SOCK_NUM,SOCKn_MR_UDP);
// phy.setSocketSourcePort(PHY_RX_SOCK_NUM,port+1);
// // Enable recv interrupt only
// phy.setSocketInterruptMaskReg(PHY_RX_SOCK_NUM,0x04);
// phy.setSocketCommandReg(PHY_RX_SOCK_NUM,SOCKn_CR_OPEN);
// phy.setSocketCommandReg(PHY_RX_SOCK_NUM,SOCKn_CR_RECV);
// }
bool UdpManager::newReceivedPackets() { return !rxBuffer->empty(); }
void UdpManager::sendPacketTo(const uint8_t* ip, const uint16_t port,
const void* data, size_t len)
{
packet_header_t header;
header.ipAddress = *(reinterpret_cast<const uint32_t*>(ip));
header.port = port;
header.payloadSize = len;
bool wasEmpty = txBuffer->empty();
bool ok = txBuffer->push(header, reinterpret_cast<const uint8_t*>(data));
if (!ok)
{
// TODO: better failure logging
puts("UDP->sendPacketTo: failed to enqueue the new packet\n");
}
else if (wasEmpty)
{
bool pok =
evtQueue.postNonBlocking(bind(&UdpManager::tx_handler, this));
if (!pok)
{
// TODO: better failure logging
puts(
"UDP->sendPacketTo: job queue full."
"Failed to post tx_handler.\n");
}
}
}
size_t UdpManager::recvPacketSize()
{
if (rxBuffer->empty())
return 0;
packet_header_t hdr = rxBuffer->getHeader();
return hdr.payloadSize;
}
void UdpManager::readPacket(uint8_t* ip, uint16_t& port, void* data)
{
packet_header_t header = rxBuffer->getHeader();
// IP address is 4 bytes long
memcpy(ip, reinterpret_cast<uint8_t*>(&(header.ipAddress)), 4);
port = header.port;
// The copy of the exact number of bytes is guaranteed by getData. We only
// have to be sure that the receiving buffer has the right capacity,
// which is given by UdpManager::recvPacketSize()
rxBuffer->getData(reinterpret_cast<uint8_t*>(data));
rxBuffer->popFront();
}
/** Interrupt and event handlers **/
void UdpManager::phyIrqHandler()
{
bool hppw = false;
uint8_t sockInt = phy.readSocketInterruptReg();
// TX socket interrupts management
if (sockInt & (0x01 << PHY_TX_SOCK_NUM))
{
// Stopping watchdog inside an IRQ is safe to do
wdt->stop();
uint8_t txFlags = phy.getSocketInterruptReg(PHY_TX_SOCK_NUM);
phy.clearSocketInterruptReg(PHY_TX_SOCK_NUM);
// Send OK interrupt flag set
if (txFlags & 0x10)
evtQueue.IRQpost(bind(&UdpManager::tx_end_handler, this), hppw);
// Timeout flag set, problems with ARP
if (txFlags & 0x08)
evtQueue.IRQpost(bind(&UdpManager::timeout_handler, this), hppw);
}
// RX socket interrupts management
if (sockInt & (0x01 << PHY_RX_SOCK_NUM))
{
uint8_t rxFlags = phy.getSocketInterruptReg(PHY_RX_SOCK_NUM);
phy.clearSocketInterruptReg(PHY_RX_SOCK_NUM);
if (rxFlags & 0x04)
evtQueue.IRQpost(bind(&UdpManager::rx_handler, this), hppw);
}
if (hppw)
Scheduler::IRQfindNextThread();
}
void UdpManager::wdtIrqHandler()
{
bool hppw = false;
evtQueue.IRQpost(bind(&UdpManager::timeout_handler, this), hppw);
if (hppw)
Scheduler::IRQfindNextThread();
}
void UdpManager::tx_handler()
{
if (txBuffer->empty())
return;
packet_header_t header = txBuffer->getHeader();
uint8_t* addr = reinterpret_cast<uint8_t*>(&header.ipAddress);
uint8_t payload[header.payloadSize];
txBuffer->getData(payload);
txBuffer->popFront();
phy.setSocketDestIp(PHY_TX_SOCK_NUM, addr);
phy.setSocketDestPort(PHY_TX_SOCK_NUM, header.port);
phy.writeData(PHY_TX_SOCK_NUM, payload, header.payloadSize);
phy.setSocketCommandReg(PHY_TX_SOCK_NUM, SOCKn_CR_SEND);
wdt->clear();
wdt->start();
}
void UdpManager::tx_end_handler()
{
if (txBuffer->empty())
return;
bool ok = evtQueue.postNonBlocking(bind(&UdpManager::tx_handler, this));
if (!ok)
{
// TODO: better failure logging
puts("UDP->tx_end_handler:job queue full, failed to post tx_handler");
}
}
void UdpManager::timeout_handler()
{
if (wdt->expired())
puts("Tx timeout due to watchdog expiration");
else
puts("Tx timeout due to phy error");
bool ok = evtQueue.postNonBlocking(bind(&UdpManager::tx_end_handler, this));
if (!ok)
puts(
"UDP->timeout_handler: job queue full, "
"failed to post tx_end_handler");
}
void UdpManager::rx_handler()
{
// get new packet len, in bytes
uint16_t len = phy.getReceivedSize(PHY_RX_SOCK_NUM);
uint8_t buffer[len];
// read all the packet, that is made of so30urceIp + sourcePort +
// payload len + payload. Since the header length is of 8 bytes,
// the payload length is len - 8 bytes! :-)
phy.readData(PHY_RX_SOCK_NUM, buffer, len);
// Set back to listening mode
phy.setSocketCommandReg(PHY_RX_SOCK_NUM, SOCKn_CR_RECV);
packet_header_t header;
memcpy(reinterpret_cast<uint8_t*>(&(header.ipAddress)), buffer, 4);
header.port = (buffer[4] << 8) | buffer[5];
header.payloadSize = len - 8;
bool pushOk = rxBuffer->push(header, &buffer[8]);
if (!pushOk)
puts(
"rx_handler error: rxBuffer is full,"
"cannot enqueue a new packet!");
}
/* Copyright (c) 2016-2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <Common.h>
#include <Singleton.h>
#include <W5200/w5200.h>
#include <e20/e20.h>
#include "PacketBuffer.h"
#include "WatchdogTimer.h"
class UdpManager : Singleton<UdpManager>
{
friend class Singleton<UdpManager>;
public:
/**
* Open a transmitting UDP socket which source port is @param port
*/
void setTxPort(uint16_t port);
/**
* Open a receiving UDP socket that will listen on @param port, listening
* starts as soon as this function is called
*/
void setRxPort(uint16_t port);
/**
* Send an UDP packet.
* @param ip: destination IP address
* @param port: destination port
* @param data: pointer to payload data
* @param len: payload lenght in bytes
*/
void sendPacketTo(const uint8_t* ip, const uint16_t port, const void* data,
size_t len);
/**
* @return true if there are new packets in the rx buffer
*/
bool newReceivedPackets();
/**
* @return payload size of the packet stored at the head of the internal
* buffer, in other words the packet that will be read if readPacket() is
* called.
*/
size_t recvPacketSize();
/**
* Read the packet placed at rx buffer's head popping it out of the queue.
* @param ip: source ip address
* @param port: source port
* @param data: pointer to a buffer in which store the payload data. It must
* have a size greater than or equal to that returned by recvPacketSize()
*/
void readPacket(uint8_t* ip, uint16_t& port, void* data);
/**
* IRQ handler for interrupts coming from the phy interface chip
* WARNING: DON'T call this!!
*/
void phyIrqHandler();
/**
* IRQ handler for interrupts coming from the tx watchdog timer
* WARNING: DON'T call this!!
*/
void wdtIrqHandler();
private:
// TX timeout time, in ms
static constexpr unsigned int TX_TIMEOUT = 500; // 500ms timeout
// Size of the event queue, used for event handling
static constexpr unsigned int EVT_QUEUE_SIZE = 20;
// Size of buffer used to store packets still to be sent, IN BYTES
static constexpr unsigned int TX_BUF_SIZE = 100000;
// Size of buffer used to store packets received, IN BYTES
static constexpr unsigned int RX_BUF_SIZE = 100000;
// TX and RX socket number, used by the phy interface
static constexpr uint8_t PHY_TX_SOCK_NUM = 0;
static constexpr uint8_t PHY_RX_SOCK_NUM = 1;
miosix::FixedEventQueue<EVT_QUEUE_SIZE> evtQueue;
PacketBuffer* txBuffer;
PacketBuffer* rxBuffer;
W5200& phy = W5200::instance();
WatchdogTimer* wdt = Singleton<WatchdogTimer>::getInstance();
// Function used by the event management thread
friend void _evt_mgmt_thread(void* args);
void tx_handler(); //< Tx event handler function
void tx_end_handler(); //< Tx end event handler function
void rx_handler(); //< New packet Rx handler function
void timeout_handler(); //< Tx timeout handler function
UdpManager();
UdpManager(const UdpManager& other);
UdpManager& operator=(const UdpManager& other);
~UdpManager();
};
/* Copyright (c) 2015-2016 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "spi_impl.h"
using namespace miosix;
void Spi_init()
{
eth::sck::mode(Mode::ALTERNATE);
eth::sck::alternateFunction(5);
eth::miso::mode(Mode::ALTERNATE);
eth::miso::alternateFunction(5);
eth::mosi::mode(Mode::ALTERNATE);
eth::mosi::alternateFunction(5);
eth::cs::mode(Mode::OUTPUT);
RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
RCC_SYNC();
// APB bus frequency is 90MHz, so leaving the BR[2:0] bits
// in CR1 makes the SPI clock frequency to be 45MHz
SPI5->CR1 = SPI_CR1_SSM // CS handled in software
| SPI_CR1_SSI // Internal CS high
| SPI_CR1_SPE // SPI enabled
| SPI_CR1_MSTR; // Master mode
}
unsigned char Spi_sendRecv(unsigned char data)
{
SPI5->DR = data;
while ((SPI5->SR & SPI_SR_RXNE) == 0)
; // Wait
return SPI5->DR;
}
void Spi_CS_high() { eth::cs::high(); }
void Spi_CS_low() { eth::cs::low(); }
/* Copyright (c) 2015-2016 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "w5200.h"
#include "spi_impl.h"
#include "w5200_defs.h"
W5200& w5200 = W5200::instance();
W5200::W5200()
{
/* initialize RX and TX buffer size vectors to default value,
which is 2kB */
std::fill(txBufSize, txBufSize + MAX_SOCK_NUM, 0x02 << 10);
std::fill(rxBufSize, rxBufSize + MAX_SOCK_NUM, 0x02 << 10);
Spi_init(); // start SPI bus if needed
setMacAddress(macAddress);
}
W5200& W5200::instance()
{
static W5200 instance;
return instance;
}
void W5200::setMacAddress(const uint8_t* address)
{
writeRegister(SHAR_BASE, address[0]);
writeRegister(SHAR_BASE + 1, address[1]);
writeRegister(SHAR_BASE + 2, address[2]);
writeRegister(SHAR_BASE + 3, address[3]);
writeRegister(SHAR_BASE + 4, address[4]);
writeRegister(SHAR_BASE + 5, address[5]);
}
void W5200::setIpAddress(const uint8_t* address)
{
writeRegister(SIPR_BASE, address[0]);
writeRegister(SIPR_BASE + 1, address[1]);
writeRegister(SIPR_BASE + 2, address[2]);
writeRegister(SIPR_BASE + 3, address[3]);
}
void W5200::setSubnetMask(const uint8_t* mask)
{
writeRegister(SUBR_BASE, mask[0]);
writeRegister(SUBR_BASE + 1, mask[1]);
writeRegister(SUBR_BASE + 2, mask[2]);
writeRegister(SUBR_BASE + 3, mask[3]);
}
void W5200::setGatewayAddress(const uint8_t* address)
{
writeRegister(GAR_BASE, address[0]);
writeRegister(GAR_BASE + 1, address[1]);
writeRegister(GAR_BASE + 2, address[2]);
writeRegister(GAR_BASE + 3, address[3]);
}
void W5200::setModeReg(uint8_t value) { writeRegister(MR, value); }
void W5200::setInterruptMask(uint8_t mask) { writeRegister(IR_MASK, mask); }
uint8_t W5200::readInterruptReg() const { return readRegister(IR); }
void W5200::setSocketMSS(SOCKET sockNum, uint16_t value)
{
writeRegister(SOCKn_MSSR0 + sockNum * SR_SIZE,
static_cast<uint8_t>((value & 0xff00) >> 8));
writeRegister(SOCKn_MSSR0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(value & 0x00ff));
}
void W5200::setRetryCount(uint16_t value) { writeRegister(RCR, value); }
void W5200::setRetryTime(uint16_t value)
{
writeRegister(RTR_BASE, static_cast<uint8_t>((value & 0xff00) >> 8));
writeRegister(RTR_BASE + 1, static_cast<uint8_t>(value & 0x00ff));
}
void W5200::setSocketModeReg(SOCKET sockNum, uint8_t value)
{
writeRegister(SOCKn_MR + sockNum * SR_SIZE, value);
}
uint8_t W5200::getSocketStatusReg(SOCKET sockNum) const
{
return readRegister(SOCKn_SR + sockNum * SR_SIZE);
}
uint8_t W5200::getSocketInterruptReg(SOCKET sockNum) const
{
return readRegister(SOCKn_IR + sockNum * SR_SIZE);
}
void W5200::clearSocketInterruptReg(SOCKET sockNum)
{
writeRegister(SOCKn_IR + sockNum * SR_SIZE, 0xFF);
}
void W5200::setSocketProtocolValue(SOCKET sockNum, uint8_t value)
{
writeRegister(SOCKn_PROTO + sockNum * SR_SIZE, value);
}
uint8_t W5200::getPhyStatus() const { return readRegister(PHY); }
uint8_t W5200::readSocketInterruptReg() const { return readRegister(SOCK_IR); }
void W5200::setSocketInterruptMask(uint8_t mask)
{
writeRegister(SOCK_IR_MASK, mask);
}
void W5200::setSocketInterruptMaskReg(SOCKET sockNum, uint8_t value)
{
writeRegister(SOCKn_IMR + sockNum * SR_SIZE, value);
}
void W5200::setSocketCommandReg(SOCKET sockNum, uint8_t value)
{
writeRegister(SOCKn_CR + sockNum * SR_SIZE, value);
}
uint8_t W5200::getSocketCommandReg(SOCKET sockNum) const
{
return readRegister(SOCKn_CR + sockNum * SR_SIZE);
}
void W5200::setSocketDestIp(SOCKET sockNum, const uint8_t* destIP)
{
writeRegister(SOCKn_DIPR0 + sockNum * SR_SIZE, destIP[0]);
writeRegister(SOCKn_DIPR0 + sockNum * SR_SIZE + 1, destIP[1]);
writeRegister(SOCKn_DIPR0 + sockNum * SR_SIZE + 2, destIP[2]);
writeRegister(SOCKn_DIPR0 + sockNum * SR_SIZE + 3, destIP[3]);
}
void W5200::setSocketDestMac(SOCKET sockNum, uint8_t* destMAC)
{
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE, destMAC[0]);
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE + 1, destMAC[1]);
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE + 2, destMAC[2]);
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE + 3, destMAC[3]);
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE + 4, destMAC[4]);
writeRegister(SOCKn_DHAR0 + sockNum * SR_SIZE + 5, destMAC[5]);
}
void W5200::setSocketDestPort(SOCKET sockNum, uint16_t destPort)
{
writeRegister(SOCKn_DPORT0 + sockNum * SR_SIZE,
static_cast<uint8_t>((destPort & 0xff00) >> 8));
writeRegister(SOCKn_DPORT0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(destPort & 0x00ff));
}
void W5200::setSocketSourcePort(SOCKET sockNum, uint16_t port)
{
writeRegister(SOCKn_SPORT0 + sockNum * SR_SIZE,
static_cast<uint8_t>((port & 0xff00) >> 8));
writeRegister(SOCKn_SPORT0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(port & 0x00ff));
}
void W5200::setSocketFragmentValue(SOCKET sockNum, uint16_t value)
{
writeRegister(SOCKn_FRAG0 + sockNum * SR_SIZE,
static_cast<uint8_t>((value & 0xff00) >> 8));
writeRegister(SOCKn_FRAG0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(value & 0x00ff));
}
void W5200::setSocketTos(SOCKET sockNum, uint8_t TOSvalue)
{
writeRegister(SOCKn_TOS + sockNum * SR_SIZE, TOSvalue);
}
void W5200::setSocketTtl(SOCKET sockNum, uint8_t TTLvalue)
{
writeRegister(SOCKn_TTL + sockNum * SR_SIZE, TTLvalue);
}
void W5200::setSocketRxMemSize(SOCKET sockNum, uint8_t memSize)
{
writeRegister(SOCKn_RXMEM_SIZE + sockNum * SR_SIZE, memSize);
rxBufSize[sockNum] = memSize << 10;
}
void W5200::setSocketTxMemSize(SOCKET sockNum, uint8_t memSize)
{
writeRegister(SOCKn_TXMEM_SIZE + sockNum * SR_SIZE, memSize);
txBufSize[sockNum] = memSize << 10;
}
uint16_t W5200::getReceivedSize(SOCKET sockNum) const
{
uint16_t len;
len = readRegister(SOCKn_RX_RSR0 + sockNum * SR_SIZE) << 8;
len += readRegister(SOCKn_RX_RSR0 + sockNum * SR_SIZE + 1);
return len;
}
void W5200::readData(SOCKET sockNum, uint8_t* data, uint16_t len)
{
uint16_t readPtr;
// Read read pointer's upper byte
readPtr = readRegister(SOCKn_RX_RD0 + sockNum * SR_SIZE) << 8;
// Read read pointer's lower byte
readPtr += readRegister(SOCKn_RX_RD0 + sockNum * SR_SIZE + 1);
readRxBuf(sockNum, readPtr, data, len);
readPtr += len;
// Update read pointer value
writeRegister(SOCKn_RX_RD0 + sockNum * SR_SIZE,
static_cast<uint8_t>((readPtr & 0xFF00) >> 8));
writeRegister(SOCKn_RX_RD0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(readPtr & 0x00FF));
}
void W5200::writeData(SOCKET sockNum, const uint8_t* data, uint16_t len)
{
uint16_t writePtr;
// Read write pointer's upper byte
writePtr = readRegister(SOCKn_TX_WR0 + sockNum * SR_SIZE) << 8;
// Read write pointer's lower byte
writePtr += readRegister(SOCKn_TX_WR0 + sockNum * SR_SIZE + 1);
writeTxBuf(sockNum, data, writePtr, len);
writePtr += len;
// Update write pointer value
writeRegister(SOCKn_TX_WR0 + sockNum * SR_SIZE,
static_cast<uint8_t>((writePtr & 0xFF00) >> 8));
writeRegister(SOCKn_TX_WR0 + sockNum * SR_SIZE + 1,
static_cast<uint8_t>(writePtr & 0x00FF));
}
void W5200::readRxBuf(SOCKET socket, uint16_t src, uint8_t* dst, uint16_t len)
{
/* Compute socket's buffer base address as a sum of RX_BUF_BASE and
* the sizes of buffers allotted for sockets before this
*/
uint16_t sockBufBase = RX_BUF_BASE;
for (int i = 0; i < socket; i++)
sockBufBase += rxBufSize[i];
/* The address mask value is equal to socket's size in byte minus one */
uint16_t mask = rxBufSize[socket] - 1;
/* The physical address at which reading process begins is base address
* plus the logical and between src pointer and address mask
*/
uint16_t startAddress = (src & mask) + sockBufBase;
if ((src & mask) + len > rxBufSize[socket])
{
uint16_t size = rxBufSize[socket] - (src & mask);
readBuffer(startAddress, dst, size);
dst += size;
size = len - size;
readBuffer(sockBufBase, dst, size);
}
else
{
readBuffer(startAddress, dst, len);
}
}
void W5200::writeTxBuf(SOCKET socket, const uint8_t* src, uint16_t dst,
uint16_t len)
{
/* Compute socket's buffer base address as a sum of RX_BUF_BASE and
* the sizes of buffers allotted for sockets before this
*/
uint16_t sockBufBase = TX_BUF_BASE;
for (int i = 0; i < socket; i++)
sockBufBase += txBufSize[i];
/* The address mask value is equal to socket's size in byte minus one */
uint16_t mask = txBufSize[socket] - 1;
/* The physical address at which reading process begins is base address
* plus the logical and between src pointer and address mask
*/
uint16_t startAddress = (dst & mask) + sockBufBase;
if ((dst & mask) + len > txBufSize[socket])
{
uint16_t size = txBufSize[socket] - (dst & mask);
writeBuffer(startAddress, src, size);
src += size;
size = len - size;
writeBuffer(sockBufBase, src, size);
}
else
{
writeBuffer(startAddress, src, len);
}
}
uint8_t W5200::readRegister(uint16_t address) const
{
uint8_t data;
Spi_CS_low();
Spi_sendRecv((address & 0xFF00) >> 8); // Address byte 1
Spi_sendRecv(address & 0x00FF); // Address byte 2
Spi_sendRecv(0x00); // Data read command and len[0]
Spi_sendRecv(0x01); // Read len[1]
data = Spi_sendRecv(0x00); // Data read
Spi_CS_high();
return data;
}
void W5200::readBuffer(uint16_t address, uint8_t* data, uint16_t len)
{
if (len == 0)
return;
Spi_CS_low();
Spi_sendRecv((address & 0xFF00) >> 8); // Address byte 1
Spi_sendRecv(address & 0x00FF); // Address byte 2
Spi_sendRecv(0x00 | ((len & 0x7F00) >> 8)); // Write cmd and len[0]
Spi_sendRecv(len & 0x00FF); // Write data len[1]
for (uint16_t i = 0; i < len; i++)
data[i] = Spi_sendRecv(0x00);
Spi_CS_high();
}
void W5200::writeBuffer(uint16_t address, const uint8_t* data, uint16_t len)
{
if (len == 0)
return;
Spi_CS_low();
Spi_sendRecv((address & 0xFF00) >> 8); // Address byte 1
Spi_sendRecv(address & 0x00FF); // Address byte 2
Spi_sendRecv(0x80 | ((len & 0x7F00) >> 8)); // Write cmd and len[0]
Spi_sendRecv(len & 0x00FF); // Write data len[1]
for (uint16_t i = 0; i < len; i++)
Spi_sendRecv(data[i]);
Spi_CS_high();
}
void W5200::writeRegister(uint16_t address, uint8_t data)
{
Spi_CS_low();
Spi_sendRecv((address & 0xFF00) >> 8); // Address byte 1
Spi_sendRecv(address & 0x00FF); // Address byte 2
Spi_sendRecv(0x80); // Write cmd and len[0]
Spi_sendRecv(0x01); // Write data len[1]
Spi_sendRecv(data); // Data write
Spi_CS_high();
}
/* Copyright (c) 2015-2016 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include "spi_impl.h"
#include "w5200_defs.h"
typedef uint8_t SOCKET;
// extern W5200& w5200; //a W5200 driver class instance
class W5200
{
public:
/**
* \return a singleton instance of W5200 class
*/
static W5200 &instance();
/**
* Set chip's MAC address
*/
void setMacAddress(const uint8_t *address);
/**
* Set chip's IP address
*/
void setIpAddress(const uint8_t *address);
/**
* Set network's subnet mask
*/
void setSubnetMask(const uint8_t *mask);
/**
* Set network's gateway IP address
*/
void setGatewayAddress(const uint8_t *address);
/**
* Set Mode register flags
*/
void setModeReg(uint8_t value);
/**
* Set retransmission timeout period in units of 100us each
* for example to set timeout period to 400ms you have to set
* this register to 0x4000
*/
void setRetryTime(uint16_t value);
/**
* Configures the number of retransmissions
*/
void setRetryCount(uint16_t value);
/**
* Configures the interrupt mask for socket interrupts
*/
void setInterruptMask(uint8_t mask);
/**
* \return chip's interrupt register value
*/
uint8_t readInterruptReg() const;
/**
* \return chip's IR2 register value, please refer to datasheet
*/
uint8_t readSocketInterruptReg() const;
/**
* Configures chip's IMR2 register, please refer to datasheet
*/
void setSocketInterruptMask(uint8_t mask);
/**
* \return value of register that indicates chip's
* physical status
*/
uint8_t getPhyStatus() const;
/**
* Writes socket's mode register
* \param sockNum: socket number, between 0 and 7
* \param value: value to be written
*/
void setSocketModeReg(SOCKET sockNum, uint8_t value);
/**
* Used to send a command to a socket through its command register
* \param sockNum: socket number, between 0 and 7
* \param value: command opcode
*/
void setSocketCommandReg(SOCKET sockNum, uint8_t value);
/**
* Used to get register's value, as a way to check if a given
* command is completed
* \param sockNum: socket number, between 0 and 7
* \return register's value
*/
uint8_t getSocketCommandReg(SOCKET sockNum) const;
/**
* Configures the socket interrupts that will be signalled
* \param sockNum: socket number, between 0 and 7
* \param value: mask value
*/
void setSocketInterruptMaskReg(SOCKET sockNum, uint8_t value);
/**
* Reads socket's interrupt register
* \param sockNum: socket number, between 0 and 7
* \return socket's interrupt register value
*/
uint8_t getSocketInterruptReg(SOCKET sockNum) const;
/**
* Resets all the socket's interrupt register flags
* \param sockNum: socket number, between 0 and 7
*/
void clearSocketInterruptReg(SOCKET sockNum);
/**
* Reads socket's status register
* \param sockNum: socket number, between 0 and 7
* \return socket's status register value
*/
uint8_t getSocketStatusReg(SOCKET sockNum) const;
/**
* Sets socket's source port value, both for TCP and UDP
* \param sockNum: socket number, between 0 and 7
* \param port: socket's source port number
*/
void setSocketSourcePort(SOCKET sockNum, uint16_t port);
/**
* Sets socket's destination MAC address
* \param sockNum: socket number, between 0 and 7
* \param destMAC: socket's destination MAC address
*/
void setSocketDestMac(SOCKET sockNum, uint8_t *destMAC);
/**
* Sets socket's destination IP address
* \param sockNum: socket number, between 0 and 7
* \param destIP: socket's destination IP address
*/
void setSocketDestIp(SOCKET sockNum, const uint8_t *destIP);
/**
* Sets socket's destination port number
* \param sockNum: socket number, between 0 and 7
* \param value: socket's destination port number
*/
void setSocketDestPort(SOCKET sockNum, uint16_t destPort);
/**
* Set socket's Maximum Segment Size for TCP mode
* \param sockNum: socket number, between 0 and 7
* \param value: MSS value
*/
void setSocketMSS(SOCKET sockNum, uint16_t value);
/**
* Set socket's protocol number when used in IPraw mode
* \param sockNum: socket number, between 0 and 7
* \param value: protocol number
*/
void setSocketProtocolValue(SOCKET sockNum, uint8_t value);
/** W5200();
* Sets Type Of Service field value in socket's IP header
* \param sockNum: socket number, between 0 and 7
* \param TOSvalue: field value
*/
void setSocketTos(SOCKET sockNum, uint8_t TOSvalue);
/**
* Sets Time To Live field value in socket's IP header
* \param sockNum: socket number, between 0 and 7
* \param TTLvalue: field value
*/
void setSocketTtl(SOCKET sockNum, uint8_t TTLvalue);
/**
* Sets Fragment field value in socket's IP header
* \param sockNum: socket number, between 0 and 7
* \param value: field value
*/
void setSocketFragmentValue(SOCKET sockNum, uint16_t value);
/**
* Configures socket's internal RX memory size
* Accepted values are: 0, 1, 2, 4, 8 and 16kB
* \param sockNum: socket number, between 0 and 7
* \param memSize: desired RX memory size
*/
void setSocketRxMemSize(SOCKET sockNum, uint8_t memSize);
/**
* Configures socket's internal TX memory size
* Accepted values are: 0, 1, 2, 4, 8 and 16kB
* \param sockNum: socket number, between 0 and 7
* \param memSize: desired TX memory size
*/
void setSocketTxMemSize(SOCKET sockNum, uint8_t memSize);
/**
* Writes data into socket TX buffer and updates in-chip pointer
* \param sockNum: socket number, between 0 and 7
* \param data: pointer to buffer containing data to be written
* \param len: number of bytes to be written
*/
void writeData(SOCKET sockNum, const uint8_t *data, uint16_t len);
/**
* Reads data from socket RX buffer and updates in-chip pointer
* \param sockNum: socket number, between 0 and 7
* \param data: pointer to buffer in which write data
* \param len: number of bytes to be read
*/
void readData(SOCKET sockNum, uint8_t *data, uint16_t len);
/**
* \param sockNum: socket number, between 0 and 7
* \return the received data size in byte
*/
uint16_t getReceivedSize(SOCKET sockNum) const;
private:
W5200();
/**
* Write one byte into chip's register
* \param address: register's address
* \param data: data to be written
*/
void writeRegister(uint16_t address, uint8_t data);
/**
* Write multiple bytes into chip's memory
* \param address: writing process start point address
* \param data: pointer to the data to be written
* \param len: number of bytes to be written
*/
void writeBuffer(uint16_t address, const uint8_t *data, uint16_t len);
/**
* Read one byte from chip's register
* \param address: register's address
* \return data read
*/
uint8_t readRegister(uint16_t address) const;
/**
* Read multiple bytes into chip's memory
* \param address: reading process start point address
* \param data: pointer to the data to be read
* \param len: number of bytes to be read
*/
void readBuffer(uint16_t address, uint8_t *data, uint16_t len);
/**
* This function is used to copy data from application buffer to socket's
* in-chip TX buffer
*
* TODO: better description
*
* \param socket: socket number, between 0 and 7
* \param src: pointer to source buffer
* \param dst: destination buffer start address.
* This start address is referred to chip's buffer!!
* \param len: number of bytes to be copied
*/
void writeTxBuf(SOCKET socket, const uint8_t *src, uint16_t dst,
uint16_t len);
/**
* This function is used to copy data from socket's in-chip TX buffer
* to application buffer
*
* TODO: better description
*
* \param socket: socket number, between 0 and 7
* \param src: source buffer start address.
* This start address is referred to chip's buffer!!
* \param dst: pointer to destination buffer
* \param len: number of bytes to be copied
*/
void readRxBuf(SOCKET socket, uint16_t src, uint8_t *dst, uint16_t len);
uint16_t txBufSize[MAX_SOCK_NUM]; // sockets TX buffer size in byte
uint16_t rxBufSize[MAX_SOCK_NUM]; // sockets RX buffer size in byte
const uint8_t macAddress[6] = {0xde, 0xad, 0x00, 0x00, 0xbe, 0xef};
};
/* Copyright (c) 2015-2016 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <Common.h>
/* Maximum number of sockets managed by the device */
constexpr uint8_t MAX_SOCK_NUM = 8;
static constexpr uint32_t COMMON_BASE = 0x0000;
// TX buffer memory base address
constexpr uint32_t TX_BUF_BASE = COMMON_BASE + 0x8000;
// RX buffer memory base address
constexpr uint32_t RX_BUF_BASE = COMMON_BASE + 0xC000;
// clang-format off
/** Common registers **/
enum eW5200Registers
{
MR = COMMON_BASE + 0x0000, //mode register address
GAR_BASE = COMMON_BASE + 0x0001, //Gateway IP Register base address
SUBR_BASE = COMMON_BASE + 0x0005, //Subnet mask Register base address
SHAR_BASE = COMMON_BASE + 0x0009, //Source MAC Register base address
SIPR_BASE = COMMON_BASE + 0x000F, //Source IP Register base address
IR = COMMON_BASE + 0x0015, //Interrupt Register address
IR_MASK = COMMON_BASE + 0x0036, //Interrupt mask register
RTR_BASE = COMMON_BASE + 0x0017, //retransmission Timeout register
RCR = COMMON_BASE + 0x0019, //retransmission count register
SOCK_IR = COMMON_BASE + 0x0034, //Socket Interrupt Register
SOCK_IR_MASK = COMMON_BASE + 0x0016, //Socket Interrupt mask register
PHY = COMMON_BASE + 0x0035, //PHY Status Register
VERSION = COMMON_BASE + 0x001F, //chip version number register
PPP_AUTH_REG = COMMON_BASE + 0x001C, //autentication type in PPPoE mode
PPP_TIME_REG = COMMON_BASE + 0x0028, //LCP Request Timer in PPPoE mode
PPP_MAGIC_REG = COMMON_BASE + 0x0029, //PPP LCP Magic number in PPPoE mode
INTLEVEL0 = COMMON_BASE + 0x0030, //set Interrupt LL timer register
INTLEVEL1 = COMMON_BASE + 0x0031,
};
/* MODE register values */
enum eW5200MODERegisters
{
MR_RST = 0x80, // Reset
MR_PB = 0x10, // Ping block enable
MR_PPPOE = 0x08, // PPPoE enable
};
/* IR register values */
enum eW5200IRRegisters
{
IR_CONFLICT = 0x80, //IP conflict
IR_PPPoE = 0x20, //PPPoE connection close
};
/* Socket registers */
enum eW5200SockRegisters
{
SR_BASE = COMMON_BASE + 0x4000, //Socket registers base address
SR_SIZE = 0x100, //Size of each channel register map
};
enum eW5200SockNRegisters
{
SOCKn_MR = SR_BASE + 0x0000, // Mode register
SOCKn_CR = SR_BASE + 0x0001, // Command register
SOCKn_IR = SR_BASE + 0x0002, // Interrupt register
SOCKn_SR = SR_BASE + 0x0003, // Status register
SOCKn_SPORT0 = SR_BASE + 0x0004, // Source port register
SOCKn_DHAR0 = SR_BASE + 0x0006, // Destination MAC address register
SOCKn_DIPR0 = SR_BASE + 0x000C, // Destination IP address register
SOCKn_DPORT0 = SR_BASE + 0x0010, // Destination port register
SOCKn_IMR = SR_BASE + 0x002C, // Interrupt mask register
SOCKn_MSSR0 = SR_BASE + 0x0012, // MSS in TCP mode
SOCKn_PROTO = SR_BASE + 0x0014, // Protocol number in IPRAW mode
SOCKn_TOS = SR_BASE + 0x0015, // IP header ToS field value
SOCKn_TTL = SR_BASE + 0x0016, // IP header TTL field value
SOCKn_FRAG0 = SR_BASE + 0x002D, // IP header Fragment field value
SOCKn_RXMEM_SIZE = SR_BASE + 0x001E, // RX buffer size register
SOCKn_TXMEM_SIZE = SR_BASE + 0x001F, // TX buffer size register
SOCKn_TX_FSR0 = SR_BASE + 0x0020, // TX buffer free size register
SOCKn_TX_RD0 = SR_BASE + 0x0022, // TX buffer read pointer address
SOCKn_TX_WR0 = SR_BASE + 0x0024, // TX buffer write pointer address
SOCKn_RX_RSR0 = SR_BASE + 0x0026, // Received data size register
SOCKn_RX_RD0 = SR_BASE + 0x0028, // RX buffer read pointer address
SOCKn_RX_WR0 = SR_BASE + 0x002A, // RX buffer write pointer address
};
/* SOCKn_MR values */
enum eW5200SockNMRValues
{
SOCKn_MR_CLOSE = 0x00, // Socket closed
SOCKn_MR_TCP = 0x01, // TCP mode
SOCKn_MR_UDP = 0x02, // UDP mode
SOCKn_MR_IPRAW = 0x03, // IP layer raw socket
SOCKn_MR_MACRAW = 0x04, // MAC layer raw socket
SOCKn_MR_PPPOE = 0x05, // PPPoE mode
SOCKn_MR_ND = 0x20, // No delayed ACK enable
SOCKn_MR_MULTI = 0x80, // Enable multicasting (only in UDP mode)
};
/* SOCKn_CR values */
enum eW5200SockNCRValues
{
SOCKn_CR_OPEN = 0x01, // Initialize and open socket
SOCKn_CR_LISTEN = 0x02, // Wait conn request in TCP mode (Server mode)
SOCKn_CR_CONNECT = 0x04, // Send conn request in TCP mode (Client mode)
SOCKn_CR_DISCON = 0x08, // Disconnect request in TCP mode
SOCKn_CR_CLOSE = 0x10, // Close socket
SOCKn_CR_SEND = 0x20, // Send all data stored in TX buffer
SOCKn_CR_SEND_MAC = 0x21, // Send data + MAC without ARP (only in UDP mode)
SOCKn_CR_SEND_KEEP = 0x22, // Check if TCP connection is still alive
SOCKn_CR_RECV = 0x40, // Receive data
};
/* SOCKn_IR values */
enum eW5200SockNIRValues
{
SOCKn_IR_CON = 0x01, // Connection established
SOCKn_IR_DISCON = 0x02, // Disconnected (TCP mode)
SOCKn_IR_RECV = 0x04, // Some data received
SOCKn_IR_TIMEOUT = 0x08, // Timeout occurred in ARP or TCP
SOCKn_IR_SEND_OK = 0x10, // SEND command completed
};
/* SOCKn_SR values */
enum eW5200SockStatus
{
SOCK_CLOSED = 0x00, // Socket closed
SOCK_INIT = 0x13, // TCP init state
SOCK_LISTEN = 0x14, // TCP server listen for connection state
SOCK_SYNSENT = 0x15, // TCP connection request sent to server
SOCK_SYNRECV = 0x16, // TCP connection request received from client
SOCK_ESTABLISHED = 0x17, // TCP connection established
SOCK_FIN_WAIT = 0x18, // TCP closing state
SOCK_CLOSING = 0x1A, // TCP closing state
SOCK_TIME_WAIT = 0x1B, // TCP closing state
SOCK_CLOSE_WAIT = 0x1C, // TCP closing state
SOCK_LAST_ACK = 0x1D, // TCP closing state
SOCK_UDP = 0x22, // Socket opened in UDP mode
SOCK_IPRAW = 0x32, // Socket opened in IP raw mode
SOCK_MACRAW = 0x42, // Socket opened in MAC raw mode
SOCK_PPPOE = 0x5F, // Socket opened in PPPoE mode
};
/* IPProto values */
enum eW5200IPProtoValues
{
IPPROTO_IP = 0, // Dummy for IP
IPPROTO_ICMP = 1, // ICMP protocol
IPPROTO_IGMP = 2, // IGMP protocol
IPPROTO_GGP = 3, // GGP protocol
IPPROTO_TCP = 6, // TCP
IPPROTO_PUP = 12, // PUP
IPPROTO_UDP = 17, // UDP
IPPROTO_IDP = 22, // XNS idp
IPPROTO_RAW = 255, // Raw IP packet */
};
// clang-format on
/* Copyright (c) 2016-2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#include "WatchdogTimer.h"
using namespace miosix;
using namespace std;
void __attribute__((naked)) TIM7_IRQHandler()
{
saveContext();
asm volatile("bl _Z8Irq_implv");
restoreContext();
}
void __attribute__((used)) Irq_impl()
{
TIM7->SR = 0;
auto inst = Singleton<WatchdogTimer>::getInstance();
inst->stop();
inst->triggered = true;
inst->irqCallback();
}
WatchdogTimer::WatchdogTimer() : ms_to_tick(1), triggered(false)
{
{
FastInterruptDisableLock dLock;
RCC->APB1ENR |= RCC_APB1ENR_TIM7EN;
RCC_SYNC();
}
unsigned int busFreq = SystemCoreClock; // CMSIS defined variable
if (RCC->CFGR & RCC_CFGR_PPRE1_2)
{
busFreq /= (0x01 << ((RCC->CFGR >> 10) & 0x3));
}
uint32_t prescaler = busFreq / 1000; // We want a 1ms/tick timer
// If we obtain a prescaler value upper than 65536 there is a problem:
// since prescaler value can be at most 65536 the counter will run at a
// frequency greater than 1kHz. To obtain a timer with a resolution of
// 1ms we have to determine how many ticks are 1ms. To do this we calculate
// the timer frequency as busFreq/prescaler and then ms_to_tick as
// timer_freq/1000
if (prescaler >= 65536)
{
prescaler = 65536;
unsigned int fclock = busFreq / prescaler;
ms_to_tick = static_cast<float>(fclock) / 1000.0f;
}
TIM7->CNT = 0;
TIM7->ARR = 0;
TIM7->PSC = prescaler - 1; // Set prescaler
TIM7->CR1 |= TIM_CR1_OPM // Enable one pulse mode
| TIM_CR1_URS; // interrupt only on counter overflow
TIM7->DIER |= TIM_DIER_UIE; // Enable interrupt
TIM7->EGR |= TIM_EGR_UG; // Generate update event to preset registers
NVIC_SetPriority(TIM7_IRQn, 10);
NVIC_ClearPendingIRQ(TIM7_IRQn);
NVIC_EnableIRQ(TIM7_IRQn);
}
WatchdogTimer::~WatchdogTimer()
{
stop();
{
FastInterruptDisableLock dLock;
RCC->APB1ENR &= ~RCC_APB1ENR_TIM7EN;
RCC_SYNC();
}
}
void WatchdogTimer::clear()
{
TIM7->CNT = 0;
triggered = false;
}
void WatchdogTimer::start()
{
triggered = false;
TIM7->CR1 |= TIM_CR1_CEN;
}
void WatchdogTimer::stop() { TIM7->CR1 &= ~TIM_CR1_CEN; }
void WatchdogTimer::setDuration(uint16_t duration)
{
float ticks = static_cast<float>(duration) * ms_to_tick;
TIM7->ARR = static_cast<uint16_t>(ticks + 0.5f); // round to nearest
}
This diff is collapsed.
This diff is collapsed.
/* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include "Common.h"
namespace modbus
{
static constexpr uint8_t EXC_ILLEGAL_FUN = 0x01; ///< Illegal function
static constexpr uint8_t EXC_ILLEGAL_ADDR = 0x02; ///< Illegal data address
static constexpr uint8_t EXC_ILLEGAL_VAL = 0x03; ///< Illegal data value
static constexpr uint8_t EXC_SLAVE_FAILURE = 0x04; ///< Slave internal failure
static constexpr uint8_t ACKNOWLEDGE = 0x05; ///< Acknowledge
static constexpr uint8_t SLAVE_BUSY = 0x06; ///< Slave busy
static constexpr uint8_t EXC_MEM_PARITY = 0x08; ///< Memory parity error
} // namespace modbus
/* Copyright (c) 2017 Skyward Experimental Rocketry
* Author: Silvano Seva
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
#pragma once
#include <slave/SlaveEngine.h>
#include <slave/SlaveInterface.h>
#include "ExceptionCodes.h"
#include "PDU.h"