#include <nuttx/config.h>
#include "app_config.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <limits.h>
#include "chip.h"
#include "stm32.h"
#include <chip/stm32_can.h>
#include "nvic.h"
#include "bl_macros.h"
#include "driver.h"
#include "crc.h"
#include "timer.h"
#include <arch/board/board.h>
#define INAK_TIMEOUT 65535
#define CAN_1MBAUD_SJW 0
#define CAN_1MBAUD_BS1 6u
#define CAN_1MBAUD_BS2 0
#define CAN_1MBAUD_PRESCALER 4u
#define CAN_500KBAUD_SJW 0
#define CAN_500KBAUD_BS1 6u
#define CAN_500KBAUD_BS2 0
#define CAN_500KBAUD_PRESCALER 8u
#define CAN_250KBAUD_SJW 0
#define CAN_250KBAUD_BS1 6u
#define CAN_250KBAUD_BS2 0
#define CAN_250KBAUD_PRESCALER 16u
#define CAN_125KBAUD_SJW 0
#define CAN_125KBAUD_BS1 6u
#define CAN_125KBAUD_BS2 0
#define CAN_125KBAUD_PRESCALER 32u
#define CAN_BTR_LBK_SHIFT 30
#define WAIT_TX_READY_MS ((TIMER_HRT_CYCLES_PER_MS-(TIMER_HRT_CYCLES_PER_MS/4))
/* Number of CPU cycles for a single bit time at the supported speeds */
#define CAN_1MBAUD_BIT_CYCLES (1*(TIMER_HRT_CYCLES_PER_US))
#define CAN_500KBAUD_BIT_CYCLES (2*(TIMER_HRT_CYCLES_PER_US))
#define CAN_250KBAUD_BIT_CYCLES (4*(TIMER_HRT_CYCLES_PER_US))
#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US))
/* All the mesured samples were off byt 28 counts this implys that
* the CAN_MSR_RX is sampled on a differnt clock then the CPU */
#define CAN_BAUD_ADJUST 28
#define CAN_BAUD_TIME_IN_MS 200
#define CAN_BAUD_SAMPLES_NEEDED 32
#define CAN_BAUD_SAMPLES_DISCARDED 8
int can_init(can_speed_t speed, uint8_t mode)
{
int speedndx = speed -1;
/*
* TODO: use full-word writes to reduce the number of loads/stores.
*
* Also consider filter use -- maybe set filters for all the message types we
* want. */
const uint32_t bitrates[] = {
(CAN_125KBAUD_SJW << 24) |
(CAN_125KBAUD_BS1 << 16) |
(CAN_125KBAUD_BS2 << 20) | (CAN_125KBAUD_PRESCALER - 1),
(CAN_250KBAUD_SJW << 24) |
(CAN_250KBAUD_BS1 << 16) |
(CAN_250KBAUD_BS2 << 20) | (CAN_250KBAUD_PRESCALER - 1),
(CAN_500KBAUD_SJW << 24) |
(CAN_500KBAUD_BS1 << 16) |
(CAN_500KBAUD_BS2 << 20) | (CAN_500KBAUD_PRESCALER - 1),
(CAN_1MBAUD_SJW << 24) |
(CAN_1MBAUD_BS1 << 16) |
(CAN_1MBAUD_BS2 << 20) | (CAN_1MBAUD_PRESCALER - 1)
};
/* Remove Unknow Offset */
if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) {
return -EINVAL;
}
uint32_t timeout;
/*
* Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP
* knock down Sleep and raise CAN_MCR_INRQ
*/
putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR);
/* Wait until initialization mode is acknowledged */
for (timeout = INAK_TIMEOUT; timeout > 0; timeout--)
{
if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0)
{
/* We are in initialization mode */
break;
}
}
if (timeout < 1)
{
/*
* Initialization failed, not much we can do now other than try a normal
* startup. */
return -ETIME;
}
putreg32(bitrates[speedndx]| mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR);
putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF, STM32_CAN1_MCR);
for (timeout = INAK_TIMEOUT; timeout > 0; timeout--)
{
if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0)
{
/* We are in initialization mode */
break;
}
}
if (timeout < 1)
{
return -ETIME;
}
/*
* CAN filter initialization -- accept everything on RX FIFO 0, and only
* GetNodeInfo requests on RX FIFO 1. */
putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR);
putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */
putreg32(3, STM32_CAN1_FS1R); /* Enable 32-bit mode for filters 0 and 1 */
/* Filter 0 masks -- data type ID 551 only */
putreg32(UAVCAN_GETNODEINFO_DTID << 22, STM32_CAN1_FIR(0, 1));
/* Top 10 bits of ID */
putreg32(0xFFC00000, STM32_CAN1_FIR(0, 2));
/* Filter 1 masks -- everything is don't-care */
putreg32(0, STM32_CAN1_FIR(1, 1));
putreg32(0, STM32_CAN1_FIR(1, 2));
putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */
putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the
* rest of filters */
putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */
putreg32(0, STM32_CAN1_FMR); /* Leave init Mode */
return OK;
}
void can_tx(uint32_t message_id, size_t length,
const uint8_t * message, uint8_t mailbox)
{
uint32_t data[2];
memcpy(data, message, sizeof(data));
/*
* Just block while waiting for the mailbox. Give it an extra 0.75 ms per
* frame to avoid an issue Ben was seeing with packets going missing on a USBtin. */
uint32_t mask = CAN_TSR_TME0 << mailbox;
time_hrt_cycles_t begin = timer_hrt_read();
while (((getreg32(STM32_CAN1_TSR) & mask) == 0) ||
timer_hrt_elapsed(begin, timer_hrt_read()) < WAIT_TX_READY_MS));
putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox));
putreg32(data[0], STM32_CAN1_TDLR(mailbox));
putreg32(data[1], STM32_CAN1_TDHR(mailbox));
putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ,
STM32_CAN1_TIR(mailbox));
}
uint8_t can_rx(uint32_t * out_message_id, size_t * out_length,
uint8_t * out_message, uint8_t fifo)
{
uint32_t data[2];
uint8_t rv = 0;
const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R };
if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK)
{
rv = 1;
/* If so, process it */
*out_message_id =
getreg32(STM32_CAN1_RIR(fifo) & CAN_RIR_EXID_MASK) >>
CAN_RIR_EXID_SHIFT;
*out_length =
getreg32(STM32_CAN1_RDTR(fifo) & CAN_RDTR_DLC_MASK) >>
CAN_RDTR_DLC_SHIFT;
data[0] = getreg32(STM32_CAN1_RDLR(fifo));
data[1] = getreg32(STM32_CAN1_RDHR(fifo));
putreg32(CAN_RFR_RFOM, fifos[fifo & 1]);
memcpy(out_message, data, sizeof(data));
}
return rv;
}
static inline uint32_t read_msr_rx(void)
{
return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX;
}
static uint32_t read_msr(time_hrt_cycles_t *now)
{
__asm__ __volatile__ ("\tcpsid i\n");
*now = timer_hrt_read();
uint32_t msr = read_msr_rx();
__asm__ __volatile__ ("\tcpsie i\n");
return msr;
}
static int read_bits_times(time_hrt_cycles_t *times, size_t max)
{
uint32_t samplecnt = 0;
bl_timer_id ab_timer= timer_allocate(modeTimeout|modeStarted, CAN_BAUD_TIME_IN_MS, 0);
time_ref_t ab_ref = timer_ref(ab_timer);
uint32_t msr;
uint32_t last_msr = read_msr(times);
while(samplecnt < max && !timer_ref_expired(ab_ref))
{
do {
msr = read_msr(×[samplecnt]);
} while(!(msr ^ last_msr) && !timer_ref_expired(ab_ref));
last_msr = msr;
samplecnt++;
}
timer_free(ab_timer);
return samplecnt;
}
int can_autobaud(can_speed_t *can_speed, bl_timer_id tboot)
{
*can_speed = CAN_UNKNOWN;
volatile int attempt = 0;
/* Threshold are at 1.5 Bit times */
/*
* We are here because there was a reset or the app invoked
* the bootloader with no bit rate set.
*/
time_hrt_cycles_t bit_time;
time_hrt_cycles_t min_cycles;
int sample;
can_speed_t speed = CAN_125KBAUD;
time_hrt_cycles_t samples[128];
while(1) {
while(1) {
min_cycles = ULONG_MAX;
int samplecnt = read_bits_times(samples, arraySize(samples));
if (timer_expired(tboot)) {
return CAN_BOOT_TIMEOUT;
}
if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) &
CAN_RFR_FMP_MASK) {
*can_speed = speed;
return CAN_OK;
}
if (samplecnt < CAN_BAUD_SAMPLES_NEEDED ) {
continue;
}
for (sample = 0; sample < samplecnt; sample += 2) {
bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample+1]);
if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) {
min_cycles = bit_time;
}
}
break;
}
uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES/4;
samples[1] = min_cycles;
speed = CAN_125KBAUD;
while(min_cycles < bit34 && speed < CAN_1MBAUD) {
speed++;
bit34 /= 2;
}
attempt++;
can_init(speed, CAN_Mode_Silent);
} /* while(1) */
return CAN_OK;
}
int can_speed2freq(can_speed_t speed)
{
return 1000000 >> (CAN_1MBAUD-speed);
}
can_speed_t can_freq2speed(int freq)
{
return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD);
}
