From c9eaad35dc651635766e061af8e4d859d83eb971 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 8 Jul 2022 17:20:57 +0300 Subject: [PATCH] add CANBUS_SSI --- F0:F030,F042,F072/CANBUS_SSI/Makefile | 155 ++++++ F0:F030,F042,F072/CANBUS_SSI/Readme.md | 66 +++ F0:F030,F042,F072/CANBUS_SSI/adc.c | 156 ++++++ F0:F030,F042,F072/CANBUS_SSI/adc.h | 31 ++ F0:F030,F042,F072/CANBUS_SSI/can.c | 354 ++++++++++++++ F0:F030,F042,F072/CANBUS_SSI/can.h | 87 ++++ F0:F030,F042,F072/CANBUS_SSI/can_process.c | 104 ++++ F0:F030,F042,F072/CANBUS_SSI/can_process.h | 38 ++ F0:F030,F042,F072/CANBUS_SSI/canstepper.bin | Bin 0 -> 13252 bytes F0:F030,F042,F072/CANBUS_SSI/flash.c | 221 +++++++++ F0:F030,F042,F072/CANBUS_SSI/flash.h | 53 +++ F0:F030,F042,F072/CANBUS_SSI/hardware.c | 141 ++++++ F0:F030,F042,F072/CANBUS_SSI/hardware.h | 60 +++ F0:F030,F042,F072/CANBUS_SSI/main.c | 173 +++++++ F0:F030,F042,F072/CANBUS_SSI/proto.c | 502 ++++++++++++++++++++ F0:F030,F042,F072/CANBUS_SSI/proto.h | 56 +++ F0:F030,F042,F072/CANBUS_SSI/spi.c | 129 +++++ F0:F030,F042,F072/CANBUS_SSI/spi.h | 39 ++ F0:F030,F042,F072/CANBUS_SSI/usart.c | 196 ++++++++ F0:F030,F042,F072/CANBUS_SSI/usart.h | 48 ++ F0:F030,F042,F072/CANBUS_SSI/usb.c | 181 +++++++ F0:F030,F042,F072/CANBUS_SSI/usb.h | 41 ++ F0:F030,F042,F072/CANBUS_SSI/usb_defs.h | 104 ++++ F0:F030,F042,F072/CANBUS_SSI/usb_lib.c | 473 ++++++++++++++++++ F0:F030,F042,F072/CANBUS_SSI/usb_lib.h | 189 ++++++++ 25 files changed, 3597 insertions(+) create mode 100644 F0:F030,F042,F072/CANBUS_SSI/Makefile create mode 100644 F0:F030,F042,F072/CANBUS_SSI/Readme.md create mode 100644 F0:F030,F042,F072/CANBUS_SSI/adc.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/adc.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/can.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/can.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/can_process.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/can_process.h create mode 100755 F0:F030,F042,F072/CANBUS_SSI/canstepper.bin create mode 100644 F0:F030,F042,F072/CANBUS_SSI/flash.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/flash.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/hardware.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/hardware.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/main.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/proto.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/proto.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/spi.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/spi.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usart.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usart.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usb.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usb.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usb_defs.h create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usb_lib.c create mode 100644 F0:F030,F042,F072/CANBUS_SSI/usb_lib.h diff --git a/F0:F030,F042,F072/CANBUS_SSI/Makefile b/F0:F030,F042,F072/CANBUS_SSI/Makefile new file mode 100644 index 0000000..a4f0cf7 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/Makefile @@ -0,0 +1,155 @@ +# make debug adds -DEBUG -Werror +# make ADDEFS="additional defs" +BINARY = canstepper +BOOTPORT ?= /dev/ttyUSB0 +BOOTSPEED ?= 115200 +# MCU FAMILY +FAMILY ?= F0 +# MCU code (STM32F072xx) +MCU ?= F072xB +# change this linking script depending on particular MCU model, +LDSCRIPT ?= stm32f0728.ld +DEFS = ${ADDEFS} -DVERSION=\"0.0.1\" -DUSARTNUM=1 +TARGET := RELEASE + +FP_FLAGS ?= -msoft-float +ASM_FLAGS ?= -mthumb -mcpu=cortex-m0 -march=armv6-m -mtune=cortex-m0 +ARCH_FLAGS = $(ASM_FLAGS) $(FP_FLAGS) + +############################################################################### +# Executables +#PREFIX ?= arm-none-eabi +# gcc from arm web site +PREFIX ?= /opt/bin/arm-none-eabi +RM := rm -f +RMDIR := rmdir +CC := $(PREFIX)-gcc +LD := $(PREFIX)-gcc +AR := $(PREFIX)-ar +AS := $(PREFIX)-as +SIZE := $(PREFIX)-size +OBJCOPY := $(PREFIX)-objcopy +OBJDUMP := $(PREFIX)-objdump +GDB := $(PREFIX)-gdb +STFLASH := $(shell which st-flash) +STBOOT := $(shell which stm32flash) +DFUUTIL := $(shell which dfu-util) + +############################################################################### +# Source files +OBJDIR = mk +SRC := $(wildcard *.c) +OBJS := $(addprefix $(OBJDIR)/, $(SRC:%.c=%.o)) +STARTUP = $(OBJDIR)/startup.o +OBJS += $(STARTUP) +# dependencies: we need them to recompile files if their headers-dependencies changed +DEPS := $(OBJS:.o=.d) + +INC_DIR ?= ../../inc + +INCLUDE := -I$(INC_DIR)/Fx -I$(INC_DIR)/cm +LIB_DIR := $(INC_DIR)/ld + +############################################################################### +# C flags +CFLAGS += -O2 -g -MD -D__thumb2__=1 +CFLAGS += -Wall -Wextra -Wshadow +CFLAGS += -fno-common -ffunction-sections -fdata-sections + +############################################################################### +# Linker flags +LDFLAGS += --static -nostartfiles --specs=nano.specs +LDFLAGS += -L$(LIB_DIR) +LDFLAGS += -T$(LDSCRIPT) +LDFLAGS += -Wl,-Map=$(OBJDIR)/$(BINARY).map +LDFLAGS += -Wl,--gc-sections + +############################################################################### +# Used libraries +LDLIBS += -Wl,--start-group -lc -lgcc -Wl,--end-group +LDLIBS += $(shell $(CC) $(CFLAGS) -print-libgcc-file-name) +DEFS += -DSTM32$(FAMILY) -DSTM32$(MCU) + +ELF := $(OBJDIR)/$(BINARY).elf +LIST := $(OBJDIR)/$(BINARY).list +BIN := $(BINARY).bin +HEX := $(BINARY).hex + +all: $(OBJDIR)/RELEASE bin list size +release: all + +debug: CFLAGS += -DEBUG -Werror -W -Wimplicit-function-declaration +debug: $(OBJDIR)/DEBUG bin list size + +$(OBJDIR)/DEBUG: + @rm -rf $(OBJDIR) + @mkdir $(OBJDIR) + @> $(OBJDIR)/DEBUG + @echo "TARGET: DEBUG" + echo "CFLAGS += -DEBUG -Werror -W -Wimplicit-function-declaration" > $(OBJDIR)/CFLAGS +$(OBJDIR)/RELEASE: + @rm -rf $(OBJDIR) + @mkdir $(OBJDIR) + @> $(OBJDIR)/RELEASE + @echo "TARGET: RELEASE" + echo "" > $(OBJDIR)/CFLAGS + +elf: $(ELF) +bin: $(BIN) +hex: $(HEX) +list: $(LIST) + +ifneq ($(MAKECMDGOALS),clean) +-include $(DEPS) +-include $(OBJDIR)/CFLAGS +endif + +$(OBJDIR): + mkdir $(OBJDIR) + +$(STARTUP): $(INC_DIR)/startup/vector.c + @echo " CC startup" + $(CC) $(CFLAGS) $(DEFS) $(INCLUDE) $(ARCH_FLAGS) -o $@ -c $< + +$(OBJDIR)/%.o: %.c + @echo " CC $<" + $(CC) $(CFLAGS) $(DEFS) $(INCLUDE) $(ARCH_FLAGS) -o $@ -c $< + +$(BIN): $(ELF) + @echo " OBJCOPY $(BIN)" + $(OBJCOPY) -Obinary $(ELF) $(BIN) + +$(HEX): $(ELF) + @echo " OBJCOPY $(HEX)" + $(OBJCOPY) -Oihex $(ELF) $(HEX) + +$(LIST): $(ELF) + @echo " OBJDUMP $(LIST)" + $(OBJDUMP) -S $(ELF) > $(LIST) + +$(ELF): $(OBJDIR) $(OBJS) + @echo " LD $(ELF)" + $(LD) $(LDFLAGS) $(ARCH_FLAGS) $(OBJS) $(LDLIBS) -o $(ELF) + +size: $(ELF) + $(SIZE) $(ELF) + +clean: + @echo " CLEAN" + @$(RM) $(HEX) + @$(RM) -rf $(OBJDIR) 2>/dev/null || true + + +flash: $(BIN) + @echo " FLASH $(BIN)" + $(STFLASH) --reset write $(BIN) 0x8000000 + +boot: $(BIN) + @echo " LOAD $(BIN) through bootloader" + $(STBOOT) -b$(BOOTSPEED) $(BOOTPORT) -w $(BIN) + +dfuboot: $(BIN) + @echo " LOAD $(BIN) THROUGH DFU" + $(DFUUTIL) -a0 -D $(BIN) -s 0x08000000 + +.PHONY: clean flash boot diff --git a/F0:F030,F042,F072/CANBUS_SSI/Readme.md b/F0:F030,F042,F072/CANBUS_SSI/Readme.md new file mode 100644 index 0000000..64c0605 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/Readme.md @@ -0,0 +1,66 @@ +Get data from SSI angle encoder & limit switches and send it to BTA CAN bus. + +Based on steppers-control board: + + +Development board for TMC2130/DRV8825 stepper driver modules +============================================================ + +Stepper control over CAN bus, RS-485 and USB. + +Pinout +====== + +PA0 - AIN0 (12V voltage control) AIN +PA1 - AIN1 (5V voltage control) AIN +PA3 - STEP timer +PA4 - DIR PP +PA5 - SCK - CFG1 - microstepping1 SPI/PP +PA6 - MISO - CFG0 - ~RST SPI/PP +PA7 - MOSI - CFG1 - microstepping0 SPI/PP +PA8 - Tx|Rx (RS485 direction) PP +PA9 - Tx (RS485) USART +PA10 - Rx (RS485) USART +PA11 - DM (USB) USB +PA12 - DP (USB) USB +PA13 - SWDIO (st-link) SWD +PA14 - SWCLK (st-link) SWD + +PB0 - ESW0 PUin +PB1 - ESW1 (limit switches or other inputs) PUin +PB2 - ESW2 PUin +PB8 - CAN_Rx (CAN) CAN +PB9 - CAN_Tx (CAN) CAN +PB10 - ESW3 PUin +PB12 - brdaddr0 PUin +PB13 - brdaddr1 (bits of board address switch) PUin +PB14 - brdaddr2 PUin +PB15 - brdaddr3 PUin + +PC13 - CFG6 - ~EN PP +PC14 - CFG3 - ~CS - microstepping2 PP +PC15 - ~SLEEP PP + +PF0 - VIO_on (turn ON Vdd of driver 4988 or 2130) OD +PF1 - ~FAULT (~fault output of 8825) FLin + +RS-485 +====== + +The same protocol as USB, but 1st symbol should be BRDADDR + + +CAN +=== + +Data format: big-endian. For example 0x03 0x04 0x05 0x0a means 0x0304050a. +Messages with variable width. +IN messages have ID = 0x70 | (devNo<<1), devNo - number, selected by jumpers @ board. +OUT messages have ID=IN+1. +zeros byte of data is command. All other - data. + + +TODO +==== + +Add linecoding_handler to change RS-485 speed due to USB connection settings? diff --git a/F0:F030,F042,F072/CANBUS_SSI/adc.c b/F0:F030,F042,F072/CANBUS_SSI/adc.c new file mode 100644 index 0000000..86fa11c --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/adc.c @@ -0,0 +1,156 @@ +/* + * This file is part of the TSYS_controller project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#include "adc.h" + +/** + * @brief ADC_array - array for ADC channels with median filtering: + * 0..3 - external channels + * 4 - internal Tsens + * 5 - Vref + */ +#define TSENS_CHAN (NUMBER_OF_ADC_CHANNELS-2) +#define VREF_CHAN (NUMBER_OF_ADC_CHANNELS-1) +static uint16_t ADC_array[NUMBER_OF_ADC_CHANNELS*9]; + +/* + * ADC channels: + * IN0 - V12 + * IN1 - V5 + * IN16- temperature sensor + * IN17- vref + */ +void adc_setup(){ + uint16_t ctr = 0; // 0xfff0 - more than 1.3ms + // Enable clocking + /* (1) Enable the peripheral clock of the ADC */ + /* (2) Start HSI14 RC oscillator */ + /* (3) Wait HSI14 is ready */ + RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; /* (1) */ + RCC->CR2 |= RCC_CR2_HSI14ON; /* (2) */ + while ((RCC->CR2 & RCC_CR2_HSI14RDY) == 0 && ++ctr < 0xfff0){}; /* (3) */ + // calibration + /* (1) Ensure that ADEN = 0 */ + /* (2) Clear ADEN */ + /* (3) Launch the calibration by setting ADCAL */ + /* (4) Wait until ADCAL=0 */ + if ((ADC1->CR & ADC_CR_ADEN) != 0){ /* (1) */ + ADC1->CR &= (uint32_t)(~ADC_CR_ADEN); /* (2) */ + } + ADC1->CR |= ADC_CR_ADCAL; /* (3) */ + ctr = 0; // ADC calibration time is 5.9us + while ((ADC1->CR & ADC_CR_ADCAL) != 0 && ++ctr < 0xfff0){}; /* (4) */ + // enable ADC + ctr = 0; + do{ + ADC1->CR |= ADC_CR_ADEN; + }while ((ADC1->ISR & ADC_ISR_ADRDY) == 0 && ++ctr < 0xfff0); + // configure ADC + /* (1) Select HSI14 by writing 00 in CKMODE (reset value) */ + /* (2) Select the continuous mode */ + /* (3) Select CHSEL0,1 - ADC inputs, 16,17 - t. sensor and vref */ + /* (4) Select a sampling mode of 111 i.e. 239.5 ADC clk to be greater than 17.1us */ + /* (5) Wake-up the VREFINT and Temperature sensor (only for VBAT, Temp sensor and VRefInt) */ + // ADC1->CFGR2 &= ~ADC_CFGR2_CKMODE; /* (1) */ + ADC1->CFGR1 |= ADC_CFGR1_CONT; /* (2)*/ + ADC1->CHSELR = ADC_CHSELR_CHSEL0 | ADC_CHSELR_CHSEL1 | ADC_CHSELR_CHSEL16 | ADC_CHSELR_CHSEL17; /* (3)*/ + ADC1->SMPR |= ADC_SMPR_SMP_0 | ADC_SMPR_SMP_1 | ADC_SMPR_SMP_2; /* (4) */ + ADC->CCR |= ADC_CCR_TSEN | ADC_CCR_VREFEN; /* (5) */ + // configure DMA for ADC + // DMA for AIN + /* (1) Enable the peripheral clock on DMA */ + /* (2) Enable DMA transfer on ADC and circular mode */ + /* (3) Configure the peripheral data register address */ + /* (4) Configure the memory address */ + /* (5) Configure the number of DMA tranfer to be performs on DMA channel 1 */ + /* (6) Configure increment, size, interrupts and circular mode */ + /* (7) Enable DMA Channel 1 */ + RCC->AHBENR |= RCC_AHBENR_DMA1EN; /* (1) */ + ADC1->CFGR1 |= ADC_CFGR1_DMAEN | ADC_CFGR1_DMACFG; /* (2) */ + DMA1_Channel1->CPAR = (uint32_t) (&(ADC1->DR)); /* (3) */ + DMA1_Channel1->CMAR = (uint32_t)(ADC_array); /* (4) */ + DMA1_Channel1->CNDTR = NUMBER_OF_ADC_CHANNELS * 9; /* (5) */ + DMA1_Channel1->CCR |= DMA_CCR_MINC | DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0 | DMA_CCR_CIRC; /* (6) */ + DMA1_Channel1->CCR |= DMA_CCR_EN; /* (7) */ + ADC1->CR |= ADC_CR_ADSTART; /* start the ADC conversions */ +} + + +/** + * @brief getADCval - calculate median value for `nch` channel + * @param nch - number of channel + * @return + */ +uint16_t getADCval(int nch){ + int i, addr = nch; + register uint16_t temp; +#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); } +#define PIX_SWAP(a,b) { temp=(a);(a)=(b);(b)=temp; } + uint16_t p[9]; + for(i = 0; i < 9; ++i, addr += NUMBER_OF_ADC_CHANNELS) // first we should prepare array for optmed + p[i] = ADC_array[addr]; + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[1]) ; PIX_SORT(p[3], p[4]) ; PIX_SORT(p[6], p[7]) ; + PIX_SORT(p[1], p[2]) ; PIX_SORT(p[4], p[5]) ; PIX_SORT(p[7], p[8]) ; + PIX_SORT(p[0], p[3]) ; PIX_SORT(p[5], p[8]) ; PIX_SORT(p[4], p[7]) ; + PIX_SORT(p[3], p[6]) ; PIX_SORT(p[1], p[4]) ; PIX_SORT(p[2], p[5]) ; + PIX_SORT(p[4], p[7]) ; PIX_SORT(p[4], p[2]) ; PIX_SORT(p[6], p[4]) ; + PIX_SORT(p[4], p[2]) ; + return p[4]; +#undef PIX_SORT +#undef PIX_SWAP +} + +// return MCU temperature (degrees of celsius * 10) +int32_t getMCUtemp(){ + int32_t ADval = getADCval(TSENS_CHAN); + int32_t temperature = (int32_t) *TEMP30_CAL_ADDR - ADval; + temperature *= (int32_t)(1100 - 300); + temperature /= (int32_t)(*TEMP30_CAL_ADDR - *TEMP110_CAL_ADDR); + temperature += 300; + return(temperature); +} + +// return Vdd * 100 (V) +uint32_t getVdd(){ + uint32_t vdd = ((uint32_t) *VREFINT_CAL_ADDR) * (uint32_t)330; // 3.3V + vdd /= getADCval(VREF_CHAN); + return vdd; +} + +static inline uint32_t Ufromadu(uint8_t nch, uint32_t vdd){ + uint32_t ADU = getADCval(nch); + ADU *= vdd; + ADU >>= 12; // /4096 + return ADU; +} + +/** + * @brief getUval - calculate U12/U5 + * @return array with members: + * 0 - V12 * 100V (U12 = 12Vin/4.93) + * 1 - V5 * 100V (U5 = 5Vin /2) + */ +uint16_t *getUval(){ + static uint16_t Uval[4]; + uint32_t vdd = getVdd(); + uint32_t val = Ufromadu(0, vdd) * 493; + Uval[0] = (uint16_t)(val / 100); + Uval[1] = (uint16_t)(Ufromadu(1, vdd) << 1); + return Uval; +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/adc.h b/F0:F030,F042,F072/CANBUS_SSI/adc.h new file mode 100644 index 0000000..e2f37bd --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/adc.h @@ -0,0 +1,31 @@ +/* + * This file is part of the TSYS_controller project. + * Copyright 2019 Edward V. Emelianov . + * + * 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 . + */ + +#ifndef ADC_H +#define ADC_H +#include "stm32f0.h" + +#define NUMBER_OF_ADC_CHANNELS (4) + +int32_t getMCUtemp(); +uint32_t getVdd(); +uint16_t getADCval(int nch); +void adc_setup(); +uint16_t *getUval(); + +#endif // ADC_H diff --git a/F0:F030,F042,F072/CANBUS_SSI/can.c b/F0:F030,F042,F072/CANBUS_SSI/can.c new file mode 100644 index 0000000..9ff7b48 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/can.c @@ -0,0 +1,354 @@ +/* + * geany_encoding=koi8-r + * can.c + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#include "can.h" +#include "hardware.h" +#include "proto.h" +#include "usart.h" + +#include // memcpy + +// circular buffer for received messages +static CAN_message messages[CAN_INMESSAGE_SIZE]; +static uint8_t first_free_idx = 0; // index of first empty cell +static int8_t first_nonfree_idx = -1; // index of first data cell +static uint16_t oldspeed = 100; // speed of last init + +static uint16_t CANID = 0xFFFF; +static CAN_status can_status = CAN_STOP; + +static void can_process_fifo(uint8_t fifo_num); + +//static CAN_message loc_flood_msg; +//static CAN_message *flood_msg = NULL; // == loc_flood_msg - to flood + +CAN_status CAN_get_status(){ + CAN_status st = can_status; + // give overrun message only once + if(st == CAN_FIFO_OVERRUN) can_status = CAN_READY; + return st; +} + +// push next message into buffer; return 1 if buffer overfull +static int CAN_messagebuf_push(CAN_message *msg){ + //MSG("Try to push\n"); + if(first_free_idx == first_nonfree_idx) return 1; // no free space + if(first_nonfree_idx < 0) first_nonfree_idx = 0; // first message in empty buffer + memcpy(&messages[first_free_idx++], msg, sizeof(CAN_message)); + // need to roll? + if(first_free_idx == CAN_INMESSAGE_SIZE) first_free_idx = 0; + return 0; +} + +// pop message from buffer +CAN_message *CAN_messagebuf_pop(){ + if(first_nonfree_idx < 0) return NULL; + CAN_message *msg = &messages[first_nonfree_idx++]; + if(first_nonfree_idx == CAN_INMESSAGE_SIZE) first_nonfree_idx = 0; + if(first_nonfree_idx == first_free_idx){ // buffer is empty - refresh it + first_nonfree_idx = -1; + first_free_idx = 0; + } + return msg; +} + +// get CAN address data from GPIO pins +void readCANID(){ + uint8_t CAN_addr = refreshBRDaddr(); + CANID = (CAN_ID_PREFIX & CAN_ID_MASK) | (CAN_addr << 1); +} + +uint16_t getCANID(){ + return CANID; +} + +void CAN_reinit(uint16_t speed){ + readCANID(); + CAN->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2; + RCC->APB1RSTR |= RCC_APB1RSTR_CANRST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CANRST; + CAN_setup(speed); +} + +/* +Can filtering: FSCx=0 (CAN->FS1R) -> 16-bit identifiers +MASK: FBMx=0 (CAN->FM1R), two filters (n in FR1 and n+1 in FR2) + ID: CAN->sFilterRegister[x].FRn[0..15] + MASK: CAN->sFilterRegister[x].FRn[16..31] + FR bits: STID[10:0] RTR IDE EXID[17:15] +LIST: FBMx=1, four filters (n&n+1 in FR1, n+2&n+3 in FR2) + IDn: CAN->sFilterRegister[x].FRn[0..15] + IDn+1: CAN->sFilterRegister[x].FRn[16..31] +*/ + +/* +Can timing: main freq - APB (PLL=48MHz) +segment = 1sync + TBS1 + TBS2, sample point is between TBS1 and TBS2, +so if TBS1=4 and TBS2=3, sum=8, bit sampling freq is 48/8 = 6MHz +-> to get 100kbps we need prescaler=60 + 250kbps - 24 + 500kbps - 12 + 1MBps - 6 +*/ + +// speed - in kbps +void CAN_setup(uint16_t speed){ + if(speed == 0) speed = oldspeed; + else if(speed < 50) speed = 50; + else if(speed > 3000) speed = 3000; + oldspeed = speed; + uint32_t tmout = 16000000; + if(CANID == 0xFFFF) readCANID(); + // Configure GPIO: PB8 - CAN_Rx, PB9 - CAN_Tx + /* (1) Select AF mode (10) on PB8 and PB9 */ + /* (2) AF4 for CAN signals */ + GPIOB->MODER = (GPIOB->MODER & ~(GPIO_MODER_MODER8 | GPIO_MODER_MODER9)) + | (GPIO_MODER_MODER8_AF | GPIO_MODER_MODER9_AF); /* (1) */ + GPIOB->AFR[1] = (GPIOB->AFR[1] &~ (GPIO_AFRH_AFRH0 | GPIO_AFRH_AFRH1))\ + | (4 << (0 * 4)) | (4 << (1 * 4)); /* (2) */ + /* Enable the peripheral clock CAN */ + RCC->APB1ENR |= RCC_APB1ENR_CANEN; + /* Configure CAN */ + /* (1) Enter CAN init mode to write the configuration */ + /* (2) Wait the init mode entering */ + /* (3) Exit sleep mode */ + /* (4) Normal mode, set timing to 100kb/s: TBS1 = 4, TBS2 = 3, prescaler = 60 */ + /* (5) Leave init mode */ + /* (6) Wait the init mode leaving */ + /* (13) Set error interrupts enable */ + CAN->MCR |= CAN_MCR_INRQ; /* (1) */ + while((CAN->MSR & CAN_MSR_INAK)!=CAN_MSR_INAK) /* (2) */ + { + if(--tmout == 0) break; + } + CAN->MCR &=~ CAN_MCR_SLEEP; /* (3) */ + CAN->MCR |= CAN_MCR_ABOM; /* allow automatically bus-off */ + + CAN->BTR = 2 << 20 | 3 << 16 | (6000/speed - 1); /* (4) */ + CAN->MCR &=~ CAN_MCR_INRQ; /* (5) */ + tmout = 16000000; + while((CAN->MSR & CAN_MSR_INAK)==CAN_MSR_INAK) if(--tmout == 0) break; /* (6) */ + // init filter: accept data only for this board + can_accept_one(); + + CAN->IER |= CAN_IER_ERRIE | CAN_IER_FOVIE0 | CAN_IER_FOVIE1; /* (13) */ + /* Configure IT */ + /* (14) Set priority for CAN_IRQn */ + /* (15) Enable CAN_IRQn */ + NVIC_SetPriority(CEC_CAN_IRQn, 0); /* (14) */ + NVIC_EnableIRQ(CEC_CAN_IRQn); /* (15) */ + can_status = CAN_READY; +} + +void can_proc(){ + // check for messages in FIFO0 & FIFO1 + if(CAN->RF0R & CAN_RF0R_FMP0){ + can_process_fifo(0); + } + if(CAN->RF1R & CAN_RF1R_FMP1){ + can_process_fifo(1); + } + IWDG->KR = IWDG_REFRESH; + if(CAN->ESR & (CAN_ESR_BOFF | CAN_ESR_EPVF | CAN_ESR_EWGF)){ // much errors - restart CAN BUS + SEND("\nToo much errors, restarting CAN!\n"); + SEND("Receive error counter: "); + printu((CAN->ESR & CAN_ESR_REC)>>24); + SEND("\nTransmit error counter: "); + printu((CAN->ESR & CAN_ESR_TEC)>>16); + SEND("\nLast error code: "); + int lec = (CAN->ESR & CAN_ESR_LEC) >> 4; + const char *errmsg = "No"; + switch(lec){ + case 1: errmsg = "Stuff"; break; + case 2: errmsg = "Form"; break; + case 3: errmsg = "Ack"; break; + case 4: errmsg = "Bit recessive"; break; + case 5: errmsg = "Bit dominant"; break; + case 6: errmsg = "CRC"; break; + case 7: errmsg = "(set by software)"; break; + } + SEND(errmsg); SEND(" error\n"); + if(CAN->ESR & CAN_ESR_BOFF) SEND("Bus off"); + if(CAN->ESR & CAN_ESR_EPVF) SEND("Passive error limit"); + if(CAN->ESR & CAN_ESR_EWGF) SEND("Error counter limit"); + // request abort for all mailboxes + CAN->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2; + // reset CAN bus + RCC->APB1RSTR |= RCC_APB1RSTR_CANRST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CANRST; + CAN_setup(0); + } + /* + static uint32_t lastFloodTime = 0; + if(flood_msg && (Tms - lastFloodTime) > (FLOOD_PERIOD_MS-1)){ // flood every ~5ms + lastFloodTime = Tms; + can_send(flood_msg->data, flood_msg->length, flood_msg->ID); + }*/ +} + +CAN_status can_send(uint8_t *msg, uint8_t len, uint16_t target_id){ + uint8_t mailbox = 0; + // check first free mailbox + if(CAN->TSR & (CAN_TSR_TME)){ + mailbox = (CAN->TSR & CAN_TSR_CODE) >> 24; + }else{ // no free mailboxes + return CAN_BUSY; + } + CAN_TxMailBox_TypeDef *box = &CAN->sTxMailBox[mailbox]; + uint32_t lb = 0, hb = 0; + switch(len){ + case 8: + hb |= (uint32_t)msg[7] << 24; + __attribute__((fallthrough)); + case 7: + hb |= (uint32_t)msg[6] << 16; + __attribute__((fallthrough)); + case 6: + hb |= (uint32_t)msg[5] << 8; + __attribute__((fallthrough)); + case 5: + hb |= (uint32_t)msg[4]; + __attribute__((fallthrough)); + case 4: + lb |= (uint32_t)msg[3] << 24; + __attribute__((fallthrough)); + case 3: + lb |= (uint32_t)msg[2] << 16; + __attribute__((fallthrough)); + case 2: + lb |= (uint32_t)msg[1] << 8; + __attribute__((fallthrough)); + default: + lb |= (uint32_t)msg[0]; + } + box->TDLR = lb; + box->TDHR = hb; + box->TDTR = len; + box->TIR = (target_id & 0x7FF) << 21 | CAN_TI0R_TXRQ; + return CAN_OK; +} + +/* +void can_send_dummy(){ + uint8_t msg = CMD_TOGGLE; + if(CAN_OK != can_send(&msg, 1, TARG_ID)) SEND("Bus busy!\n"); +} + +void can_send_broadcast(){ + uint8_t msg = CMD_BCAST; + if(CAN_OK != can_send(&msg, 1, BCAST_ID)) SEND("Bus busy!\n"); + MSG("Broadcast message sent\n"); +} + +void set_flood(CAN_message *msg){ + if(!msg) flood_msg = NULL; + else{ + memcpy(&loc_flood_msg, msg, sizeof(CAN_message)); + flood_msg = &loc_flood_msg; + } +}*/ + +static void can_process_fifo(uint8_t fifo_num){ + if(fifo_num > 1) return; + CAN_FIFOMailBox_TypeDef *box = &CAN->sFIFOMailBox[fifo_num]; + volatile uint32_t *RFxR = (fifo_num) ? &CAN->RF1R : &CAN->RF0R; + // read all + while(*RFxR & CAN_RF0R_FMP0){ // amount of messages pending + // CAN_RDTxR: (16-31) - timestamp, (8-15) - filter match index, (0-3) - data length + CAN_message msg; + uint8_t *dat = msg.data; + uint8_t len = box->RDTR & 0x0f; + msg.length = len; + msg.ID = box->RIR >> 21; + msg.fifoNum = fifo_num; // @parsing only data from FIFO0 will be accepted, FIFO1 is for monitoring + if(len){ // message can be without data + uint32_t hb = box->RDHR, lb = box->RDLR; + switch(len){ + case 8: + dat[7] = hb>>24; + __attribute__((fallthrough)); + case 7: + dat[6] = (hb>>16) & 0xff; + __attribute__((fallthrough)); + case 6: + dat[5] = (hb>>8) & 0xff; + __attribute__((fallthrough)); + case 5: + dat[4] = hb & 0xff; + __attribute__((fallthrough)); + case 4: + dat[3] = lb>>24; + __attribute__((fallthrough)); + case 3: + dat[2] = (lb>>16) & 0xff; + __attribute__((fallthrough)); + case 2: + dat[1] = (lb>>8) & 0xff; + __attribute__((fallthrough)); + case 1: + dat[0] = lb & 0xff; + } + } + if(CAN_messagebuf_push(&msg)) return; // error: buffer is full, try later + *RFxR |= CAN_RF0R_RFOM0; // release fifo for access to next message + } + *RFxR = 0; // clear FOVR & FULL +} + +void cec_can_isr(){ + if(CAN->RF0R & CAN_RF0R_FOVR0){ // FIFO overrun + CAN->RF0R &= ~CAN_RF0R_FOVR0; + can_status = CAN_FIFO_OVERRUN; + } + if(CAN->RF1R & CAN_RF1R_FOVR1){ + CAN->RF1R &= ~CAN_RF1R_FOVR1; + can_status = CAN_FIFO_OVERRUN; + } + if(CAN->MSR & CAN_MSR_ERRI){ // Error + CAN->MSR &= ~CAN_MSR_ERRI; + // request abort for problem mailbox + if(CAN->TSR & CAN_TSR_TERR0) CAN->TSR |= CAN_TSR_ABRQ0; + if(CAN->TSR & CAN_TSR_TERR1) CAN->TSR |= CAN_TSR_ABRQ1; + if(CAN->TSR & CAN_TSR_TERR2) CAN->TSR |= CAN_TSR_ABRQ2; + } +} + +// accept only data for given device @ FIFO0, filter 0 +void can_accept_one(){ + CAN->FMR = CAN_FMR_FINIT; // Enter filter init mode, (16-bit + mask, bank 0 for FIFO 0) + CAN->FA1R = CAN_FA1R_FACT0; // Acivate filter 0 for ID + // main data - FIFO0, filter0 + CAN->FM1R = CAN_FM1R_FBM0; // Identifier list mode + CAN->sFilterRegister[0].FR1 = (CANID << 5) | (0x8f<<16); // Set the Id list + //CAN->sFilterRegister[0].FR2 = (0x8f<<16) | 0x8f; + CAN->FMR &= ~CAN_FMR_FINIT; // Leave filter init +} +// accept everything @ FIFO1, filter 4 +void can_accept_any(){ + CAN->FMR = CAN_FMR_FINIT; + CAN->FA1R |= CAN_FA1R_FACT1; // Acivate bank 1 + CAN->FFA1R = CAN_FFA1R_FFA1; // bank 1 for FIFO1 + CAN->FM1R &= ~CAN_FM1R_FBM1; // MASK + CAN->sFilterRegister[1].FR1 = 0; // all IDs + CAN->sFilterRegister[1].FR2 = (0x8f<<16) | 0x8f; + CAN->FMR &= ~CAN_FMR_FINIT; +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/can.h b/F0:F030,F042,F072/CANBUS_SSI/can.h new file mode 100644 index 0000000..1ed4133 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/can.h @@ -0,0 +1,87 @@ +/* + * geany_encoding=koi8-r + * can.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#pragma once +#ifndef __CAN_H__ +#define __CAN_H__ + +#include "hardware.h" + +// default CAN speed - 100kbps +#define CAN_SPEED_DEFAULT 100 + +// amount of filter banks in STM32F0 +#define STM32F0FBANKNO 28 +// flood period in milliseconds +#define FLOOD_PERIOD_MS 5 + +// simple 1-byte commands +//#define CMD_TOGGLE (0xDA) +//#define CMD_BCAST (0xAD) +// mask clearing bits of board address +#define CAN_ID_MASK (0x7E0) +// prefix to make ID from any number +#define CAN_ID_PREFIX (0x100) +// "target" ID: num=0 +//#define TARG_ID (CAN_ID_PREFIX & CAN_ID_MASK) +// "broadcast" ID: all ones +//#define BCAST_ID (0x7FF) + +// incoming message buffer size +#define CAN_INMESSAGE_SIZE (8) + +// CAN message +typedef struct{ + uint8_t data[8]; // up to 8 bytes of data + uint8_t length; // data length + //uint8_t filterNo; // filter number + uint8_t fifoNum; // message FIFO number + uint16_t ID; // ID of receiver +} CAN_message; + +typedef enum{ + CAN_STOP, + CAN_READY, + CAN_BUSY, + CAN_OK, + CAN_FIFO_OVERRUN +} CAN_status; + +CAN_status CAN_get_status(); + +void readCANID(); +uint16_t getCANID(); + +void CAN_reinit(uint16_t speed); +void CAN_setup(uint16_t speed); + +CAN_status can_send(uint8_t *msg, uint8_t len, uint16_t target_id); +//void can_send_dummy(); +//void can_send_broadcast(); +void can_proc(); + +CAN_message *CAN_messagebuf_pop(); +void can_accept_any(); +void can_accept_one(); +//void set_flood(CAN_message *msg); + +#endif // __CAN_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/can_process.c b/F0:F030,F042,F072/CANBUS_SSI/can_process.c new file mode 100644 index 0000000..4e89165 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/can_process.c @@ -0,0 +1,104 @@ +/* + * geany_encoding=koi8-r + * can_process.c + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#include "adc.h" +#include "can.h" +#include "can_process.h" +#include "proto.h" + +extern volatile uint32_t Tms; // timestamp data +/* +// v==0 - send V12 & V5 +static void senduival(){ + uint8_t buf[5]; + uint16_t *vals = getUval(); + buf[0] = CMD_GETUVAL; // V12 and V5 + buf[1] = vals[0] >> 8; // H + buf[2] = vals[0] & 0xff;// L + buf[3] = vals[1] >> 8; // -//- + buf[4] = vals[1] & 0xff; + SEND_CAN(buf, 5); +} + +static void sendu16(uint8_t cmd, uint16_t data){ + uint8_t buf[3]; + buf[0] = cmd; + buf[1] = data >> 8; + buf[2] = data & 0xff; + SEND_CAN(buf, 3); +} +*/ + +void can_messages_proc(){ + CAN_message *can_mesg = CAN_messagebuf_pop(); + if(!can_mesg) return; // no data in buffer + uint8_t len = can_mesg->length; +#ifndef EBUG + if(can_mesg->fifoNum == 1){ // not my data - just show it +#endif + if(monitCAN){ + printu(Tms); + SEND(" #"); + printuhex(can_mesg->ID); + SEND(" (F#"); printu(can_mesg->fifoNum); SEND(")"); + for(uint8_t ctr = 0; ctr < len; ++ctr){ + SEND(" "); + printuhex(can_mesg->data[ctr]); + } + IWDG->KR = IWDG_REFRESH; + newline(); sendbuf(); + } +#ifndef EBUG + return; + } +#endif + IWDG->KR = IWDG_REFRESH; + /* + if(!len) return; // no data in message + uint8_t *data = can_mesg->data; + switch(data[0]){ + case CMD_PING: // pong + SEND_CAN(data, 1); + break; + case CMD_GETMCUTEMP: + sendu16(CMD_GETMCUTEMP, (int16_t)getMCUtemp()); + break; + case CMD_GETUVAL: + senduival(); + break; + case CMD_GETU3V3: + sendu16(CMD_GETU3V3, (uint16_t)getVdd()); + break; + } + */ +} + +// try to send messages, wait no more than 100ms +CAN_status try2send(uint8_t *buf, uint8_t len, uint16_t id){ + uint32_t Tstart = Tms; + while(Tms - Tstart < SEND_TIMEOUT_MS){ + if(CAN_OK == can_send(buf, len, id)) return CAN_OK; + IWDG->KR = IWDG_REFRESH; + } + SEND("CAN_BUSY\n"); + return CAN_BUSY; +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/can_process.h b/F0:F030,F042,F072/CANBUS_SSI/can_process.h new file mode 100644 index 0000000..b99f542 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/can_process.h @@ -0,0 +1,38 @@ +/* + * geany_encoding=koi8-r + * can_process.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#include "can.h" +#include "flash.h" + +// timeout for trying to send data +#define SEND_TIMEOUT_MS (10) + +// 8-bit commands sent by master +typedef enum{ + CMD_PING, // just echo it back + CMD_GETMCUTEMP, // MCU temperature value + CMD_GETUVAL, // answer with values of V12 and V5 + CMD_GETU3V3, // answer with values of V3.3 +} CAN_commands; + +void can_messages_proc(); +CAN_status try2send(uint8_t *buf, uint8_t len, uint16_t id); diff --git a/F0:F030,F042,F072/CANBUS_SSI/canstepper.bin b/F0:F030,F042,F072/CANBUS_SSI/canstepper.bin new file mode 100755 index 0000000000000000000000000000000000000000..041297ec1eff5e29451782d06768019bda30d8bf GIT binary patch literal 13252 zcmd^ldw5gVmG3%|Y|E0t@&lq5=18)^AQNQs2;otLWDb^&0S80k2bmoC0W$cZ*d|fZ zZwB#5BQTv1JGlky-q2}ghNj7k>(-qljc?p0ok`NPl`za_L()2(7Nu>Ps!3Ye_;K!U z%O+tuo#}k@-T$t?Z+XAg-h1u!+Iy`dauWZ=Wu!g@cnm=Q#UI1DHO%Ukg>!S*ygvtz z82&uYpIc+n^w}Eg)TF+ePwJ3ut*4hxH|Yj*+?=(`CmvStIc~9jwWu8DHuVmWT@mpf z*10AkoYgL#_iJ2=$a(*h?(g4+C&i^evXLjxCp zn?**_(l((V7{%tl9^lNEFuKBgYJfLC<)X_v^KszOm>&3HmuBo6=qo0rIqXUwJvWfr zAA03QAwJaan!K#0V)`hiHGdo%78nn7g9=_wJSwM&hOpLnj)wIo04buAGlkzYD&jBP zT;-4*x}g^n^3v}C(nQ^$F|6c!s8_$AGaKZ@ez}1-@JA6}a8oz*GH`Omz#rjwKgF4v zRAE;+u}zlT3T=c!#t?<{O0K~&tXIT8m`vSS7~(KqESI|TwGC8A&7lBf$<{rInMyKH zeDFSDyYLfXmq1oA@BFs$!LEMMbynyWR2D_IvRl=i(w*9!*6r~coKFcJQGJ#;skBe< zm1_OHY3&gz*zV0-w(mZ&J}J1x^lnpdE$#3t{F|M77I594b1jeji~qR)6@j|dMV8UO zp&sD@VWanQ^_PS?A$wt3H~wc_9!O=S=JG%a%F6?)Zn}`YP|5ydtXn{dF-7+wAv<_h z__}LjgjOl8aaJlw?|#5654<8gKvVi5sn zdN0|z$O3UmfNWYReOwe+TZ8u8NGfXD{`f235e9^>2^XOi!$N-}IWro_sQ`b!2*_AT zU>QVe)eUBNPTxx_bFNoVr8%V6ctcb>pwrLMkml6}a)W&?WmL9lv0qL=H$?n&jIG1w zv$Z%;9}et*EutP6gg7IuRY1vL#2V zJk;g>&fQvfWnIX&#oMcjHfh}>KF+$yqlK^0N*hH5JX6vy!JAA*6xOxn#booZE9-IF zB%A+pB{?V+@$yM`-Wi^=hLn2uxX*@N*!ap%XV`8 z{=z`Xow^Z?s43426@)eHoRO{=#lJBXDsWN@rMO=hCP;)AHAe>%D7s z8l|r{nO)h@0AwO3>StFc$A241%V+jgJ6j|cc~{P3eGpk$M)7?!?EjvbOHH(p;&0C^ zM@wWa1#7=PlaIE0=Ic$SbhDG&oLaPO)FM7h<5d*jFtZe6>t+^y#7*sU_a&l`Qlrd*H4Y4pj7-_1T3%Pt!OP&{kqMl1{E zf|;MhbSTqj{xz25gM*yUJx8<_`dI}C0L}o)?4-VkG86)Hz%yBvXB=q14(JD50t5!f z^Leo$<;(s8S4L!Md9VLjp_l8;G;#H%>J^Sr)6O$gRQ>mYf(O6yzwpR<)mpdk$a=*x zx9X8)?(YXzrJfAts|wtu!F)xLn}T~%RPH|MCsMV>houaxLvYz*6E7<*~Vm-?F9u?~e>|lE{ zy_wT>O;Xo&gSWIjtxOeuf^UGv^X{|x&yxP0sw$@MuxX})Y@XFI$mKs?px`saZtAOe zfvT%HclAAwSx!+^g?>wQ^)a-%EyT07s!UZ}RdK%REAI0Zj^r9o&=WK6VlV(O!MKgGW|eFr3;F)f{<_z$NSp`JP|JxcLo(>bWq zG~J2IqabVd2BP@nlypD0j^Zy(|9aq?uHTQS#1yXv5~29sX^Ia|{TTK|^SW~eSJfvt zO9OPq4=4p(0py8Gz3^0n?!4ZC9v44m7WIC!xWs=lZ_yRvO}iB1`|~ckGT*ToYjq}l z4PDlE)7SLR)nu&vD0=$TfGamrWxnY8?Yo>cOWfl!N`WDr7*Y%yLpD=NpO9gZO1b!7 z&ZdcaX(XohYRYn?3@aDkJNqlp_BQUeyj@UT(1nL#(g$Rqq_w2ZMHwIbiEtloyX``i zI6heAG5Ixq#ikSeJ+6D--6wJ-dHzb$NEbj+B{U~JRk*@gD#TGg=bDa=`iE23Us=nK zO8TY&JqLZtu&m`z3EXnkGQQxru)|X(hJ`ZE`O-6j+v5}(1eK%@>q2k%(=huDe=7Xe zgX3@bQ(V)L3PG{v4M|aQe*3SYOpERkjq_G~^zPHJX65)bf3Eu-iQ|?3?uIG$R7(uL z8Dn?S3UPH=o&taPhrBfs182UmX(Q;UhF`IwBu$Ql)C(D zl9k@|uXGlP+vZ^5cR*ic@g!4 zQyHjVyg81#Z%V^cTroxQR=k{sg#h%B?HfYL_B@NS4{!-URvKI=p6KU#7K#f!MromF z@GbO=Nf+vjzH8C~Xn|3B_2b=7y!vt56I?HMA=`iaV|BRJe{P4epV(XY4bVGI<7+%0 zKFWL~SF-~q%{ig39tAHR!dt&C^V2)gdx+hr8J-2A7HexIg-w()#4aiAQ;fIpx}S`y z#57oM@_xsf?4Dx$di!mnefx-hhD(j{P3z4N@4@X|={sxFc(13xGOg*JYf_DrE*hmV zl%}RMuSsRf%n96Di=$nXNgORA-9C07#=#HHlI9&rz5+rvva)p^D7jFl^j@pWvN9^> zeLpEheUnnetW5pfdbj3_`XF1Mt>qXrijVcDTgjp#?h^H67I5+ZHQgjAtr_F@Il=+E zGu^HmQ9>X^s)a8yy^xDDgOsK$0k@C%j4DE<$V z6hA(h((f2@1c?-0yl>XzBzXlc;Vhq7^+i~<(SNZFH=Ee2(?kll6jBbNIBorv7QKt~d+xpYqh;KLZWcxoEBScPkN7 z;odd)_uwWlqK5m|p9RZMT59~53taAQhLd6aOONQ!ajNZ8(P7JR?+NilS*4^rl%8r* zm^50qvW{#$k`m{T6S*?QOJ?Q7&nD!AH1p{0&+iI`<-`JLvj-z1({f_DEGK?E3$y8= zo=)+|&hx?%@dBV1a1KBh)<~jY^7k5VTK792&(9Y6x%m1CRo;>a*$Vy2eO!?;stg{*EBVwo#lJ7x{U$?A zUUpEka%;pc)nYxfsa7ycRHS;RQm7H^(pFoJL^efFg}6p?W1Q^P=uXt-#HDGKMR|$h z#u+O7y9lFVO|%U5J3OJhBq!dTRrYQ#yY5!@Q2d{gJy&LX@Wy+5f>BL9Uy3O(H-CmK zigOhIX!1(zv;8UsQ&3}nEwaF_j8+P@s1HRd1^8`2fz{rbd@%+cwQ+3L z)uH&BF{Ae79~|>`yc||rRM`LNsTbi#1113Z3>0&I@hhe zu-0IfwgiS>c}eIOCWhq1H)UWNejSq&{~*uNlo}}$DE{?XHCw~*e#}Cw-8ijEw#RRK zM=*LA9gHT&hT#F-AXk_%stulSsUzwD6>1`BWpd(qnX{%MD$ot3p!K)%&6o=1SLBal z?}IO2#N1JoA0&CC-l-ycaH~CSG}Xhc(fI9w3bR_u1y|(6U&!|0$FW;`1_n>|6a0)S zuct#DT~OdyQ`yl{T1axDQ&#l{h87mIl=7spaPhZpa$6k5Jgo?@)|Q*gAd>Vu7# zj)UULJQZondIwIpGDo%H9zZQ8Wk~)tc*|p5$DlH(AhN5X`^GDSTnX9liSYKxF^4@h%JY@M6YeT|QN(3m z6ww7KAAF8382>7n47J3SnDn}!W0RSvP#IB@V^g~QcVjxrkQ_rj!Lw%Ep!Nk3t{`oE z4;8~ZtaQ7BoP+QNHIrjASM>3iM*3y!7UsfeB9^r|rARl%_BCeFZeN(+bvkN2w}kK?m1vPgUtMO(*h(D{_;=f~@V z+*)|E=tJYYePV2%-4*Gu+oD`S$o3DrT3jZs4LZxMJ#tKeapfqtmd%@t-VKwJ69M@m zr1V$ghs|3fX{+enH{PRO*-m3`yw|Z0_t?7kW0mle$FWDFneos!emIDivHkue51Y+R z0$XJ#v}yaM>jTR%GdSMu?WK{bXN3A-U2={%&-We4ISqDuR8E}6OGQq+0B>)YRm^9T zsMo!H)nzJBQOB|8V{KUZZmhjjPN9>o4_GWMx+A`{RZ=V+yGUj-1ueW#L zp743h?Tqays|?ABmuA=sYiBv@HrPs2%paseRWxN>Vv;W8B7EWGSeZvo{CcMLPmZ}T z_U_D1_@P%Oy)h@KJ3AvM`leuqvs=(QH8Y#2#a-%|-4xr1_A!j^1N{Ex8t|R+*Na9! zb@zt9gDYbLIo3=CfM19;*8MRchF{c$?GWJ23!HW0w@LVCBQiNss|9`9q{_mHwx$j zgaBu)ms4{5R!?Bi;JjorDYWh!Dd&(g-=s8YbLu$jv1MkDd8gQf_`qj^&YX4YC1cr_ zrBP|Wr1v|}(u?|CbNyeIx(gPUA=300y1!STmvY4ysH)oGthy(}50f5W3c^jboYLLp z`=MnavJVDi@T(;Q^7aNq7c6%+EXYb|3VVwK+9FLp}R=-8}Ek00TZbXYXg6x{8#;f&Y`xW?-(oH!P>E0Yk?MDQJmu#w+_#9~> zM*JUeALZiQ&jWY>d!NMz3&a#E*i}%rtXzrw=~{`|f`WakiJY~C5LNA=3qH=4!7tst z)VnE=<3Hh6ZBV+|n#hSrv?BA}R(;5lTfVr=9?W@cq{)E%FE6ojD#EQBbbTDnLX=jU zntH`)(UxbF-5UFJb;J-E7Ikv%F1H#P-W(}DL=WQD$6K!mcFx{={h*aLa85@5S$HR- zf2k+SdC|&Z8KYEl$c$DPQ3Ilux_`DBJ>Aq*#bT7MijeIN?{)4;#ECt=h}CIXB<`VW z(#4JPMx%7CiA6oeLs@P^9d9w>OHs&Tlr-M-lq~72HDtM*;-M$&$dc~O3K;zv#iv0Z z+b^&~mS2-I4ZN7$3aW)Sa4jHFsyOXQjXQ%vma}>K*u8QTjUv%tD2( z>^{0aFl5J!=W^Z>BgYj1&AZ{V5`gYi>pq$kHdE z7q|G!_^lDO-dca}fYF&fKr2(PtG$Q-0V;1LPVTT>a1Yyhxk_j6z{fG#PtI3gmE|X% zkmaLCkwLJkM3u)FPn)S+lqUJ(ZKYH>C(^f`7Ug;cuXm8HHGk1Gi3dNPkT6|=y7Z7LpJZ>S_QmYz46oB zhWD@n*^eybsqa9hw8H4;liH#2F1foh>3c3(GsHcdv4N~L z4ami|By+KiA*))-EBIhBl}th8pX_-NT49bX-njx1lPDhsY3{;dBaTfh1>a?(tdoe;0#8MWP_unl~#!|{o z4d5=5$z~6*d?Vw>Hz0#=0VW3TfIpbq(}X<@c%kL&Dh}2f`&_y2Y%Ma376(`2+vCB! z%(4u_s{=nDScF&k_xEyZt`B_GH4){C-!v2lCWg|Id+4#pn!vvyZer2Mm*kIPx9_D0 zl0`S@Hq%N?B$WRw;rq>+ueekZV~{I81MJKmn_i;Et!mv)fo6gY+*9$ z{3x=ZoaxSokqprQ-u?<%WF@chbPwR29?zXB2)q~j1bi7XliIL^)RQQ0oq_2T&$ItzH?aMGU)M>=V|D=K<%0&E_uwFW3CH1I|^@ ziTtk)RGTjdqRyixdpb^ANw3#@vZvB~ZQx1Q#AvN>A$*UpLOg1Eb)Y)rG|=j_=+pSR zaQipFdclgUawuQxbXY{E4_{KXQobTjOd0nGE`i&mfz2r+KEcy$!FT(5p|*LCU{bw% zb>DVHlq}g|b%<7fGkP`VT%A4IH_&K4tZPZqQj0INEIa+(n%^bqc^dSjiTgvtF{kZU zv3H<{)ARheWve8Sb1QrUv*GyQzY4nrL0C~fJh(jkH^LReFZ9m~!v!zU^V^-*3U)n4 z-x8iLJs@}#oV!>m_J@JXdmeNi5Q?R-v{hF^_%6}qS~0rSUNBm%J|h@+eV6)$LcM;c zVb|A$ZKeG}CcYT$E`9tSXX)_}S)U#7hUyGwLdPYX&X5zT7kU-F13w6;vwy@3F1-lE` z4DO?wq7BMGqj4_FqQt#k=*W{EL_qcNBXgAC-TKnCx=;s%VP6 zJd%!RSYh3PcVAt2C;pSQ9B2FyFs$|!5gT}6v40XR^OlNPVUynwx+h#6dNO=IbUA!2 zl!i#FCR87^*uNdYH#Msh^I2S5AN+kR)ozWb@x~(irRX|uwm&DNu&xL8S7IDWV}QNu z*8_7rw%WVukGNa}yf4P;!d8^uj@5_lkV|nSV^o3H1FLZ*a!HR%7gbpbaJHqh56AL> zVTue}FC%}sFsiYz*MH9J;aDh{(WJ7F*9%}a(}GIVnPqiRw33hpf)vA z87o0HPEPz`W>;*vg}wWKH{(g(wagbWU%_nURg_1M$}*E}AKDWrnatNh@0gF`l&GvB zwGw46N&+ZQuR?hekPjFw!FNYwC(`i#9EK168&}%sS6wNisy=#3P6&wZbweCK6;tt( zvH0L7_~H8I+LYZ_Hh9Elld9zEUFDh9X9cl&{Z6AZ0;emL{;2Bp@Hu^mo=x#I7wVr~ zpgR6$YW7uHsf&jAVmuHyXR6B`DLTz=DYE>df5oog{=iAl3JH7f|`aplfPj{w7 ztWB!5)ctv(vs2Vp$&$xjV$B)FxNjTG-kOHR7`d!Uxtd`yHlIeH9(^pE z)MujtCv3&mut;odFotqP4mSkhhL9825#Rn>Ll!bIy)lRC^{b4Q??(R7zu-1XUV{B5 z@8Nm7W&CD5l@qw7e=Ez(wx6~l4xQ&c%k$)zY0$Y69@y49aKE`vc%0_^o;H%{-Gr0< z?<(et-=6oy%=c-;VLB(&3(itEEL!d96AqSc!n=rh-A&TmuXKh5CSN93M$cBr{5a|K zY53(uwqAEyhu=`7;$`%d|L6PP#w~o#HTe$vcEsk);tb5!glr2D6U}{bVc&4NT&b5c z5PfhZ`T)1)Jbvwf-z7j7RhP+PKu-_8r_Cn5J;mm;{O)F)6&{QaLtj|^eei*7x2He@LsV^#C0}4Nw54@$ozxGCECuYxU~mA*Js7=Lm8+58h{Rv3(y1JP9wD$Wo73P ztVkrZwYQ+4ZS{xA11+n6bkrHbflxPy}qsWzyrR9hW0Hu^QRphkD`gZz_wi- zfyTxLHrCi$-`3FB?kU|un&h!MSQQJZ$1 z;4&lHjo)eDO-i0pzQf;kq`85wYvd2Kx3%!ad}kZ)2;PN3{9(M_?`u8S*uc7x+Q2v9 ze>bPNcvT&E(t4z&uCbj-nyp=4&2Qive4Re9rL(bv_cb@SJ>1xU;q4&vk-4Ej8#JV? zm2bj$5|7Q^wVTKC2O-kdBEF-swSjMK>-0mBFu1FYX%nAh#~cb#CF9L)M*ue83~BM5 zpqgLjI9wO#U^M+Hyo2qHK4{?_>KcdR7IG!T`PoLvW3#<)(M1{{U|t8f+tT7|ZRpsj zspnULTBt_j0pF44+oo$8=SM!3oo0IwUA`loZOP*m7O!AJVRRQdR`By^9sEPSX2x;Q zp=q99;+DuY<+rb}I(cNK_s$Y;GeH_O|-^#y}_E*4q3C-|`6KkrO0;+V5+8=xeezRn{Z%vN@~frs+m;u2Pn=H7Ml<(f6D!j^4m!?fhN`Q$oDk9FH3_xYHC zGtEsJ1GXaCyS;KR4>j%twUv!?cE7hJxg}_wb7gFO2b1e<{Fqw?!i55NKs6p}s&CXd zKSP>hPKU;au7gmXcHhJNwo>=J^fYzoo|EXmXPV~VJbvBneA9T)JI56sRBH~+BY`QP zy(Q4t4mLq%hv)nE-P)!F9Z7G!8bS@+&LGCGx+5K$J;{}Q4g8uC9=6``DQl8Ck<{Yk zQQKhKOkp?j9ZV5wne997yz{R6HI-YjM8MbXYiVQ#2TNi0qv>Q@GaF~jMeV^OQ2$0g zxz3)-Rckk_)9hsvp{8(UN7~!Le7>`(rBSnwE!Y-d{sZKI#Y|VBzO4tE4j#ct(XV+F zDDwD3Sy4`Pt5b) z*7qUke0iSMzW;*eGyln0Db`9?EJp}O7faIdV=ko(>87O|Q)-AyOHnACtaXAFl+Ecl z6wc45Ds80X6oi&0b*_&MsFdld5@iLoQyWsAO>_WIk0p{Do z?!>$z(L-Iv&(J6XJ!<@f Znf + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +/** + ATTENTION!! + This things works only if you will add next section: + + .myvars : + { + . = ALIGN(1024); + __varsstart = ABSOLUTE(.); + KEEP(*(.myvars)); + } > rom + + after section .data +*/ +#include +#include "adc.h" +#include "flash.h" +#include "proto.h" // printout +#include // memcpy + +// max amount of Config records stored (will be recalculate in flashstorage_init() +static uint32_t maxCnum = FLASH_BLOCK_SIZE / sizeof(user_conf); + +#define USERCONF_INITIALIZER { \ + .userconf_sz = sizeof(user_conf) \ + ,.CANspeed = 100 \ + ,.encoderID = 8 \ + ,.limitsID = 0xe \ + } + +static int erase_flash(const void*, const void*); +static int write2flash(const void*, const void*, uint32_t); +// don't write `static` here, or get error: +// 'memcpy' forming offset 8 is out of the bounds [0, 4] of object '__varsstart' with type 'uint32_t' +const user_conf *Flash_Data = (const user_conf *)(&__varsstart); + +user_conf the_conf = USERCONF_INITIALIZER; + +static int currentconfidx = -1; // index of current configuration + +/** + * @brief binarySearch - binary search in flash for last non-empty cell + * any struct searched should have its sizeof() @ the first field!!! + * @param l - left index + * @param r - right index (should be @1 less than last index!) + * @param start - starting address + * @param stor_size - size of structure to search + * @return index of non-empty cell or -1 + */ +static int binarySearch(int r, const uint8_t *start, int stor_size){ + int l = 0; + while(r >= l){ + int mid = l + (r - l) / 2; + const uint8_t *s = start + mid * stor_size; + if(*((const uint16_t*)s) == stor_size){ + if(*((const uint16_t*)(s + stor_size)) == 0xffff){ // next is free + return mid; + }else{ // element is to the right + l = mid + 1; + } + }else{ // element is to the left + r = mid - 1; + } + } + return -1; // not found +} + +/** + * @brief flashstorage_init - initialization of user conf storage + * run in once @ start + */ +void flashstorage_init(){ + if(FLASH_SIZE > 0 && FLASH_SIZE < 20000){ + uint32_t flsz = FLASH_SIZE * 1024; // size in bytes + flsz -= (uint32_t)(&__varsstart) - FLASH_BASE; + maxCnum = flsz / sizeof(user_conf); +//SEND("flsz="); printu(flsz); +//SEND("\nmaxCnum="); printu(maxCnum); newline(); sendbuf(); + } + // -1 if there's no data at all & flash is clear; maxnum-1 if flash is full + currentconfidx = binarySearch((int)maxCnum-2, (const uint8_t*)Flash_Data, sizeof(user_conf)); + if(currentconfidx > -1){ + memcpy(&the_conf, &Flash_Data[currentconfidx], sizeof(user_conf)); + } +} + +// store new configuration +// @return 0 if all OK +int store_userconf(){ + // maxnum - 3 means that there always should be at least one empty record after last data + // for binarySearch() checking that there's nothing more after it! + if(currentconfidx > (int)maxCnum - 3){ // there's no more place + currentconfidx = 0; + if(erase_flash(Flash_Data, (&__varsstart))) return 1; + }else ++currentconfidx; // take next data position (0 - within first run after firmware flashing) + return write2flash((const void*)&Flash_Data[currentconfidx], &the_conf, sizeof(the_conf)); +} + +static int write2flash(const void *start, const void *wrdata, uint32_t stor_size){ + int ret = 0; + if (FLASH->CR & FLASH_CR_LOCK){ // unloch flash + FLASH->KEYR = FLASH_KEY1; + FLASH->KEYR = FLASH_KEY2; + } + while (FLASH->SR & FLASH_SR_BSY); + if(FLASH->SR & FLASH_SR_WRPRTERR){ + MSG("Can't remove write protection\n"); + return 1; // write protection + } + FLASH->SR = FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR; // clear all flags + FLASH->CR |= FLASH_CR_PG; + const uint16_t *data = (const uint16_t*) wrdata; + volatile uint16_t *address = (volatile uint16_t*) start; + uint32_t i, count = (stor_size + 1) / 2; + for (i = 0; i < count; ++i){ + IWDG->KR = IWDG_REFRESH; + *(volatile uint16_t*)(address + i) = data[i]; + while (FLASH->SR & FLASH_SR_BSY); + if(FLASH->SR & FLASH_SR_PGERR){ + ret = 1; // program error - meet not 0xffff + MSG("FLASH_SR_PGERR\n"); + break; + }else while (!(FLASH->SR & FLASH_SR_EOP)); + FLASH->SR = FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR; + } + FLASH->CR |= FLASH_CR_LOCK; // lock it back + FLASH->CR &= ~(FLASH_CR_PG); + MSG("Flash stored\n"); + return ret; +} + +/** + * @brief erase_flash - erase N pages of flash memory + * @param start - first address + * @param end - last address (or NULL if need to erase all flash remaining) + * @return 0 if succeed + */ +static int erase_flash(const void *start, const void *end){ + int ret = 0; + uint32_t nblocks = 1, flsz = 0; + if(!end){ // erase all remaining + if(FLASH_SIZE > 0 && FLASH_SIZE < 20000){ + flsz = FLASH_SIZE * 1024; // size in bytes + flsz -= (uint32_t)start - FLASH_BASE; + } + }else{ // erase a part + flsz = (uint32_t)end - (uint32_t)start; + } + nblocks = flsz / FLASH_BLOCK_SIZE; + if(nblocks == 0 || nblocks >= FLASH_SIZE) return 1; + for(uint32_t i = 0; i < nblocks; ++i){ +#ifdef EBUG + SEND("Try to erase page #"); printu(i); newline(); sendbuf(); +#endif + IWDG->KR = IWDG_REFRESH; + /* (1) Wait till no operation is on going */ + /* (2) Clear error & EOP bits */ + /* (3) Check that the Flash is unlocked */ + /* (4) Perform unlock sequence */ + while ((FLASH->SR & FLASH_SR_BSY) != 0){} /* (1) */ + FLASH->SR = FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR; /* (2) */ + /* if (FLASH->SR & FLASH_SR_EOP){ + FLASH->SR |= FLASH_SR_EOP; + }*/ + if ((FLASH->CR & FLASH_CR_LOCK) != 0){ /* (3) */ + FLASH->KEYR = FLASH_KEY1; /* (4) */ + FLASH->KEYR = FLASH_KEY2; + } + /* (1) Set the PER bit in the FLASH_CR register to enable page erasing */ + /* (2) Program the FLASH_AR register to select a page to erase */ + /* (3) Set the STRT bit in the FLASH_CR register to start the erasing */ + /* (4) Wait until the EOP flag in the FLASH_SR register set */ + /* (5) Clear EOP flag by software by writing EOP at 1 */ + /* (6) Reset the PER Bit to disable the page erase */ + FLASH->CR |= FLASH_CR_PER; /* (1) */ + FLASH->AR = (uint32_t)Flash_Data + i*FLASH_BLOCK_SIZE; /* (2) */ + FLASH->CR |= FLASH_CR_STRT; /* (3) */ + while(!(FLASH->SR & FLASH_SR_EOP)); + FLASH->SR |= FLASH_SR_EOP; /* (5)*/ + if(FLASH->SR & FLASH_SR_WRPRTERR){ /* Check Write protection error */ + ret = 1; + MSG("Write protection error!\n"); + FLASH->SR |= FLASH_SR_WRPRTERR; /* Clear the flag by software by writing it at 1*/ + break; + } + FLASH->CR &= ~FLASH_CR_PER; /* (6) */ + } + return ret; +} + +void dump_userconf(){ + SEND("userconf_addr="); printuhex((uint32_t)Flash_Data); + SEND("\nuserconf_sz="); printu(the_conf.userconf_sz); + SEND("\nCANspeed="); printu(the_conf.CANspeed); + SEND("\nencoderID="); printuhex(the_conf.encoderID); + SEND("\nlimitsID="); printuhex(the_conf.limitsID); + newline(); + sendbuf(); +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/flash.h b/F0:F030,F042,F072/CANBUS_SSI/flash.h new file mode 100644 index 0000000..94c72cc --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/flash.h @@ -0,0 +1,53 @@ +/* + * flash.h + * + * Copyright 2017 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +#pragma once +#ifndef __FLASH_H__ +#define __FLASH_H__ + +#include "hardware.h" + +#define FLASH_BLOCK_SIZE (1024) +#define FLASH_SIZE_REG ((uint32_t)0x1FFFF7CC) +#define FLASH_SIZE *((uint16_t*)FLASH_SIZE_REG) + +/* + * struct to save user configurations + */ +typedef struct __attribute__((packed, aligned(4))){ + uint16_t userconf_sz; // "magick number" + uint16_t CANspeed; // default CAN speed + uint16_t encoderID; // ID to send encoder data + uint16_t limitsID; // ID to send limit-switches data + uint8_t sendenc; // send encoder's measurements by CAN bus + uint8_t sendsw; // send limit switches values by CAN bus +} user_conf; + +extern user_conf the_conf; // global user config (read from FLASH to RAM) +// data from ld-file: start address of storage +extern const uint32_t __varsstart; + +void flashstorage_init(); +int store_userconf(); +void dump_userconf(); + +#endif // __FLASH_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/hardware.c b/F0:F030,F042,F072/CANBUS_SSI/hardware.c new file mode 100644 index 0000000..61b34f1 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/hardware.c @@ -0,0 +1,141 @@ +/* + * This file is part of the Stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "hardware.h" +#include "proto.h" + +static uint8_t brdADDR = 0; + +void Jump2Boot(){ + void (*SysMemBootJump)(void); + volatile uint32_t addr = 0x1FFFC800; + // reset systick + SysTick->CTRL = 0; + // reset clocks + RCC->APB1RSTR = RCC_APB1RSTR_CECRST | RCC_APB1RSTR_DACRST | RCC_APB1RSTR_PWRRST | RCC_APB1RSTR_CRSRST | + RCC_APB1RSTR_CANRST | RCC_APB1RSTR_USBRST | RCC_APB1RSTR_I2C2RST | RCC_APB1RSTR_I2C1RST | + RCC_APB1RSTR_USART4RST | RCC_APB1RSTR_USART3RST | RCC_APB1RSTR_USART2RST | RCC_APB1RSTR_SPI2RST | + RCC_APB1RSTR_WWDGRST | RCC_APB1RSTR_TIM14RST | RCC_APB1RSTR_TIM7RST | RCC_APB1RSTR_TIM6RST | + RCC_APB1RSTR_TIM3RST | RCC_APB1RSTR_TIM2RST; + RCC->APB2RSTR = RCC_APB2RSTR_DBGMCURST | RCC_APB2RSTR_TIM17RST | RCC_APB2RSTR_TIM16RST | RCC_APB2RSTR_TIM15RST | + RCC_APB2RSTR_USART1RST | RCC_APB2RSTR_SPI1RST | RCC_APB2RSTR_TIM1RST | RCC_APB2RSTR_ADCRST | RCC_APB2RSTR_SYSCFGRST; + RCC->AHBRSTR = 0; + RCC->APB1RSTR = 0; + RCC->APB2RSTR = 0; + // remap memory to 0 (only for STM32F0) + SYSCFG->CFGR1 = 0x01; __DSB(); __ISB(); + SysMemBootJump = (void (*)(void)) (*((uint32_t *)(addr + 4))); + // set main stack pointer + __set_MSP(*((uint32_t *)addr)); + // jump to bootloader + SysMemBootJump(); +} + +void iwdg_setup(){ + uint32_t tmout = 16000000; + /* Enable the peripheral clock RTC */ + /* (1) Enable the LSI (40kHz) */ + /* (2) Wait while it is not ready */ + RCC->CSR |= RCC_CSR_LSION; /* (1) */ + while((RCC->CSR & RCC_CSR_LSIRDY) != RCC_CSR_LSIRDY){if(--tmout == 0) break;} /* (2) */ + /* Configure IWDG */ + /* (1) Activate IWDG (not needed if done in option bytes) */ + /* (2) Enable write access to IWDG registers */ + /* (3) Set prescaler by 64 (1.6ms for each tick) */ + /* (4) Set reload value to have a rollover each 2s */ + /* (5) Check if flags are reset */ + /* (6) Refresh counter */ + IWDG->KR = IWDG_START; /* (1) */ + IWDG->KR = IWDG_WRITE_ACCESS; /* (2) */ + IWDG->PR = IWDG_PR_PR_1; /* (3) */ + IWDG->RLR = 1250; /* (4) */ + tmout = 16000000; + while(IWDG->SR){if(--tmout == 0) break;} /* (5) */ + IWDG->KR = IWDG_REFRESH; /* (6) */ +} + +/* +MODER - input/output/alternate/analog +OTYPER - pushpull/opendrain +OSPEEDR - low(x0)/med(01)/high(11) +PUPDR - no/pullup/pulldown/reserved +AFRL, AFRH - alternate fno +*/ +void gpio_setup(){ + // here we turn on clocking for all GPIO used + RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOCEN | RCC_AHBENR_GPIOFEN + | RCC_AHBENR_DMAEN; + // PA. AIN: PA0, PA1, PA8 - Tx/Rx + GPIOA->MODER = GPIO_MODER_MODER8_O | GPIO_MODER_MODER1_AI | GPIO_MODER_MODER0_AI; + GPIOA->PUPDR = 0; + GPIOA->OTYPER = 0; + // PB. PB12..15 - board address, pullup input; PB0..2, PB10 - ESW, pullup inputs (inverse) + GPIOB->MODER = 0; + GPIOB->PUPDR = GPIO_PUPDR0_PU | GPIO_PUPDR1_PU | GPIO_PUPDR2_PU | GPIO_PUPDR10_PU | + GPIO_PUPDR12_PU | GPIO_PUPDR13_PU | GPIO_PUPDR14_PU | GPIO_PUPDR15_PU; + GPIOB->OTYPER = 0; + // other pins will be set up later + brdADDR = READ_BRD_ADDR(); +} + +// setup TIM2 to work as downcounting 32-bit timer (2mks period) +void tim2_Setup(){ + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // enable clocking + TIM2->CR1 = 0; // turn off timer + TIM2->PSC = 95; // 500kHz + TIM2->ARR = 0xffffff; // 24 bit counter from 0xffffff to 0 + // turn it on, downcounting + TIM2->CR1 = TIM_CR1_CEN | TIM_CR1_DIR; +} + +/* +// PA3 (STEP): TIM15_CH2; 48MHz -> 1MHz +void timer_setup(){ + // PA3 - Tim15Ch2 (AF0) + GPIOA->AFR[0] = (GPIOA->AFR[0] & ~GPIO_AFRL_AFRL3); + GPIOA->MODER = (GPIOA->MODER & ~GPIO_MODER_MODER3) | GPIO_MODER_MODER3_AF; // set alternate output + RCC->APB2ENR |= RCC_APB2ENR_TIM15EN; // enable clocking + TIM15->CR1 = 0; // turn off timer + TIM15->CCMR1 = TIM_CCMR1_OC2M_2; // Force inactive + TIM15->PSC = TIM15PSC; + TIM15->CCR2 = TIM15CCR2; + TIM15->ARR = 1000; // this value will be changed later + TIM15->CCER = TIM_CCER_CC2E; // enable PWM out + TIM15->BDTR = TIM_BDTR_MOE; // enable main output + // enable IRQ & update values + TIM15->EGR = TIM_EGR_UG; + TIM15->DIER = TIM_DIER_CC2IE; + NVIC_EnableIRQ(TIM15_IRQn); + NVIC_SetPriority(TIM15_IRQn, 0); +#if 0 + TIM15->CCMR1 = TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1; // PWM mode 1: active->inacive + TIM15->BDTR = TIM_BDTR_MOE; + TIM15->CR1 = TIM_CR1_CEN; +#endif + MSG("Timer is ON\n"); +}*/ + +uint8_t refreshBRDaddr(){ + return (brdADDR = READ_BRD_ADDR()); +} +uint8_t getBRDaddr(){return brdADDR;} + +void sleep(uint16_t ms){ + uint32_t Tnew = Tms + ms; + while(Tnew != Tms) nop(); +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/hardware.h b/F0:F030,F042,F072/CANBUS_SSI/hardware.h new file mode 100644 index 0000000..9853843 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/hardware.h @@ -0,0 +1,60 @@ +/* + * This file is part of the Stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef __HARDWARE_H__ +#define __HARDWARE_H__ +// Most of hardware-dependendent definitions & functions +#include + +// refresh encoder data each 70ms +#define ENCODER_PERIOD (69) + +#define CONCAT(a,b) a ## b +#define STR_HELPER(s) #s +#define STR(s) STR_HELPER(s) + +#ifndef USARTNUM +#error "Define USARTNUM 1 or 2" +#endif +#define FORMUSART(X) CONCAT(USART, X) +#define USARTX FORMUSART(USARTNUM) + +// Board address - PB15|14|13|12 +#define READ_BRD_ADDR() ((GPIOB->IDR >> 12) & 0x0f) + +// RS-485 receive/transmit (PA8: 0-Rx, 1-Tx) +#define RS485_TX() do{GPIOA->BSRR = GPIO_BSRR_BS_8;}while(0) +#define RS485_RX() do{GPIOA->BRR = GPIO_BRR_BR_8;}while(0) + +// pins manipulation +// end-switches state +#define ESW_STATE() ((GPIOB->IDR & 0x07) | ((GPIOB->IDR>>7) & 0x08)) + +extern volatile uint32_t Tms; + +void Jump2Boot(); +void gpio_setup(); +void iwdg_setup(); +uint8_t getBRDaddr(); +uint8_t refreshBRDaddr(); +void sleep(uint16_t ms); +//void timer_setup(); +void tim2_Setup(); + +#endif // __HARDWARE_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/main.c b/F0:F030,F042,F072/CANBUS_SSI/main.c new file mode 100644 index 0000000..dab95fd --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/main.c @@ -0,0 +1,173 @@ +/* + * main.c + * + * Copyright 2017 Edward V. Emelianoff + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#include "adc.h" +#include "can.h" +#include "can_process.h" +#include "flash.h" +#include "hardware.h" +#include "proto.h" +#include "spi.h" +#include "usart.h" +#include "usb.h" +#include "usb_lib.h" + +#include + +volatile uint32_t Tms = 0; + +/* Called when systick fires */ +void sys_tick_handler(void){ + ++Tms; +} + +static void uhex(uint8_t *arr, uint8_t l){ + if(l > 4 || l == 0) return; + char buf[13] = "0x"; + int8_t i, j, bidx = 2; + for(i = 0; i < l; ++i, ++arr){ + for(j = 1; j > -1; --j){ + uint8_t half = (*arr >> (4*j)) & 0x0f; + if(half < 10) buf[bidx++] = half + '0'; + else buf[bidx++] = half - 10 + 'a'; + } + } + buf[bidx++] = '\n'; + buf[bidx] = 0; + SEND(buf); + sendbuf(); +} + +/* +static void print32bits(uint8_t *arr){ + uint8_t _16[2]; + _16[0] = (arr[0]>>4)&3; _16[1] = (arr[0]<<4) | (arr[1] >> 4); + uhex(_16, 2); +}*/ + +#define USBBUF 63 +// usb getline +static char *get_USB(){ + static char tmpbuf[USBBUF+1], *curptr = tmpbuf; + static int rest = USBBUF; + uint8_t x = USB_receive((uint8_t*)curptr); + if(!x) return NULL; + curptr[x] = 0; + if(x == 1 && *curptr == 0x7f){ // backspace + if(curptr > tmpbuf){ + --curptr; + USND("\b \b"); + } + return NULL; + } + USB_sendstr(curptr); // echo + if(curptr[x-1] == '\n'){ + curptr = tmpbuf; + rest = USBBUF; + // omit empty lines + if(tmpbuf[0] == '\n') return NULL; + // and wrong empty lines + if(tmpbuf[0] == '\r' && tmpbuf[1] == '\n') return NULL; + return tmpbuf; + } + curptr += x; rest -= x; + if(rest <= 0){ // buffer overflow + curptr = tmpbuf; + rest = USBBUF; + } + return NULL; +} + +// send encoder data +static void CANsendEnc(uint8_t *buf){ + uint32_t ctr = TIM2->CNT; + uint8_t msg[8]; + memcpy(msg, buf, 4); + msg[4] = 0; + msg[5] = (ctr >> 16) & 0xff; + msg[6] = (ctr >> 8 ) & 0xff; + msg[7] = (ctr >> 0 ) & 0xff; + can_send(msg, 8, the_conf.encoderID); +} +// send limit-switches data +static void CANsendLim(){ + uint8_t msg[8] = {0}; + msg[2] = ESW_STATE(); + can_send(msg, 8, the_conf.limitsID); +} + +int main(void){ + uint32_t lastT = 0, // send buffer time + encT = 0 // send encoder & limit-switches data time + ; + sysreset(); + SysTick_Config(6000, 1); + gpio_setup(); // + read board address + usart_setup(); + adc_setup(); + flashstorage_init(); + CAN_setup(the_conf.CANspeed); + USB_setup(); + spi_setup(); + tim2_Setup(); + iwdg_setup(); + + while (1){ + IWDG->KR = IWDG_REFRESH; // refresh watchdog + if(Tms - lastT > 499){ + sendbuf(); + lastT = Tms; + } + if((the_conf.sendenc || the_conf.sendsw) && Tms - encT > ENCODER_PERIOD){ + encT = Tms; + if(the_conf.sendenc) SPI_transmit(NULL, 4); + if(the_conf.sendsw) CANsendLim(); + } + can_proc(); + usb_proc(); + usart_proc(); // switch RS-485 to Rx after last byte sent + char *txt; + if((txt = get_USB())){ + cmd_parser(txt, TARGET_USB); + } + IWDG->KR = IWDG_REFRESH; + if(usartrx()){ // usart1 received data, store in in buffer + usart_getline(&txt); + cmd_parser(txt, TARGET_USART); + } + IWDG->KR = IWDG_REFRESH; + can_messages_proc(); + uint8_t buf[4]; + uint8_t a = SPI_getdata(buf, 4); + if(a){ + if(the_conf.sendenc) CANsendEnc(buf); + SEND("Inverse code: "); uhex(buf, 4); + uint32_t *_u = (uint32_t*) buf; *_u = ~*_u; + SEND("Direct code: "); uhex(buf, 4); + } +/* + if(ostctr != Tms){ // check steppers not more than once in 1ms + ostctr = Tms; + } +*/ + } + return 0; +} + diff --git a/F0:F030,F042,F072/CANBUS_SSI/proto.c b/F0:F030,F042,F072/CANBUS_SSI/proto.c new file mode 100644 index 0000000..d64aece --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/proto.c @@ -0,0 +1,502 @@ +/* + * geany_encoding=koi8-r + * proto.c + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#include "adc.h" +#include "can.h" +#include "flash.h" +#include "hardware.h" +#include "proto.h" +#include "spi.h" +#include "usart.h" +#include "usb.h" +#include // strlen, strcpy( + +extern volatile uint8_t canerror; +uint8_t monitCAN = 0; // ==1 to show CAN messages +#define BUFSZ UARTBUFSZ + +static char buff[BUFSZ+1], *bptr = buff; +static uint8_t blen = 0, // length of data in `buff` + USBcmd = 0; // ==1 if buffer prepared for USB + +char *omit_spaces(char *buf){ + while(*buf){ + if(*buf > ' ') break; + ++buf; + } + return buf; +} + +void buftgt(uint8_t isUSB){ + USBcmd = isUSB; +} + +void sendbuf(){ + IWDG->KR = IWDG_REFRESH; + if(blen == 0) return; + if(USBcmd){ + *bptr = 0; + USB_sendstr(buff); + }else usart_send_blocking(buff, blen); + bptr = buff; + blen = 0; +} + +void addtobuf(const char *txt){ + IWDG->KR = IWDG_REFRESH; + int l = strlen(txt); + if(l > BUFSZ){ + sendbuf(); + if(USBcmd) USB_sendstr(txt); + else usart_send_blocking(txt, l); + }else{ + if(blen+l > BUFSZ){ + sendbuf(); + } + strcpy(bptr, txt); + bptr += l; + } + blen += l; +} + +void bufputchar(char ch){ + if(blen > BUFSZ-1){ + sendbuf(); + } + *bptr++ = ch; + ++blen; +} + +// show all ADC values +static inline void showADCvals(){ + char msg[] = "ADCn="; + for(int n = 0; n < NUMBER_OF_ADC_CHANNELS; ++n){ + msg[3] = n + '0'; + addtobuf(msg); + printu(getADCval(n)); + newline(); + } +} + +static inline void printmcut(){ + SEND("MCUT="); + int32_t T = getMCUtemp(); + if(T < 0){ + bufputchar('-'); + T = -T; + } + printu(T); + newline(); +} + +static inline void showUIvals(){ + uint16_t *vals = getUval(); + SEND("V12="); printu(vals[0]); + SEND("\nV5="); printu(vals[1]); + SEND("\nV33="); printu(getVdd()); + newline(); +} + +// check address & return 0 if wrong or roll to next non-digit +static char *chk485addr(char *txt){ + uint32_t N; + char *nxt = getnum(txt, &N); + if(nxt == txt) return NULL; + if(N == getBRDaddr()){ + return nxt; + } + return NULL; +} + +// parse `txt` to CAN_message +static CAN_message *parseCANmsg(char *txt){ + static CAN_message canmsg; + uint32_t N; + char *n; + int ctr = -1; + canmsg.ID = 0xffff; + do{ + txt = omit_spaces(txt); + n = getnum(txt, &N); + if(txt == n) break; + txt = n; + if(ctr == -1){ + if(N > 0x7ff){ + SEND("ID should be 11-bit number!\n"); + return NULL; + } + canmsg.ID = (uint16_t)(N&0x7ff); + ctr = 0; + continue; + } + if(ctr > 7){ + SEND("ONLY 8 data bytes allowed!\n"); + return NULL; + } + if(N > 0xff){ + SEND("Every data portion is a byte!\n"); + return NULL; + } + canmsg.data[ctr++] = (uint8_t)(N&0xff); + }while(1); + if(canmsg.ID == 0xffff){ + SEND("NO ID given, send nothing!\n"); + return NULL; + } + canmsg.length = (uint8_t) ctr; + return &canmsg; +} + +// send command, format: ID (hex/bin/dec) data bytes (up to 8 bytes, space-delimeted) +TRUE_INLINE void sendCANcommand(char *txt){ + CAN_message *msg = parseCANmsg(txt); + if(!msg) return; + uint32_t N = 1000000; + while(CAN_BUSY == can_send(msg->data, msg->length, msg->ID)){ + if(--N == 0) break; + } +} + +static uint8_t userconf_changed = 0; // ==1 if user_conf was changed +TRUE_INLINE void userconf_manip(char *txt){ + txt = omit_spaces(txt); + switch(*txt){ + case 'd': // dump + dump_userconf(); + break; + case 's': // store + if(userconf_changed){ + if(!store_userconf()){ + userconf_changed = 0; + SEND("Stored!"); + }else SEND("Error when storing!"); + } + break; + default: + SEND("\nUserconf commands:\n" + "d - userconf dump\n" + "s - userconf store\n" + ); + } +} + +// Change CAN IDs +static void chID(uint32_t new, uint16_t *id){ + uint16_t I = (uint16_t)new; + if(I < 1 || I > 0x7FF){ + SEND("ID should be from 1 to 0x7FF"); + return; + } + if(*id != I){ + *id = I; + SEND("ID changed to "); printu(I); + userconf_changed = 1; + } +} + +// change CAN send flags +static void chCAN(char val, uint8_t *ch){ + if(val != '0' && val != '1'){ + SEND("Point 0 or 1"); + return; + } + val -= '0'; + if(*ch != val){ + *ch = val; + if(val) SEND("ON"); + else SEND("OFF"); + userconf_changed = 1; + } +} + +// a set of setters for user_conf +TRUE_INLINE void setters(char *txt){ + uint32_t U; + txt = omit_spaces(txt); + if(!*txt){ + SEND("Setters need more arguments"); + return; + } + char *nxt = getnum(txt + 1, &U); + switch(*txt){ + case 'c': // set CAN speed + if(nxt == txt + 1){ + SEND("No CAN speed given"); + return; + } + if(U < 50){ + SEND("Speed should be not less than 50kbps"); + return; + } + if(U > 3000){ + SEND("Speed should be not greater than 3000kbps"); + return; + } + if(the_conf.CANspeed != (uint16_t)U){ + the_conf.CANspeed = (uint16_t)U; + SEND("Set CAN speed to "); printu(U); + userconf_changed = 1; + } + break; + case 'e': + if(nxt == txt + 1){ + SEND("No ID given"); + return; + } + chID(U, &the_conf.encoderID); + break; + case 'E': + txt = omit_spaces(txt + 1); + chCAN(*txt, &the_conf.sendenc); + break; + case 'l': + if(nxt == txt + 1){ + SEND("No ID given"); + return; + } + chID(U, &the_conf.limitsID); + break; + case 'L': + txt = omit_spaces(txt + 1); + chCAN(*txt, &the_conf.sendsw); + break; + default: + SEND("\nSetters commands:\n" + "c - set default CAN speed\n" + "e - set encoderID\n" + "Ex - autosend (1) or not (0) encoders val to CAN\n" + "l - set limitsID\n" + "Lx - autosend/not limit switches values to CAN\n" + ); + } +} + +/** + * @brief cmd_parser - command parsing + * @param txt - buffer with commands & data + * @param isUSB - == 1 if data got from USB + */ +void cmd_parser(char *txt, uint8_t isUSB){ + sendbuf(); + USBcmd = isUSB; + // we can't simple use &txt[p] as variable: it can be non-aligned by 4!!! + if(isUSB == TARGET_USART){ // check address and roll message to nearest non-space + txt = chk485addr(txt); + if(!txt) return; + } + txt = omit_spaces(txt); + // long commands, commands with arguments + switch(*txt){ + case 's': + sendCANcommand(txt + 1); + goto eof; + break; + case 'S': // setters + setters(txt + 1); + goto eof; + break; + case 'U': + userconf_manip(txt + 1); + goto eof; + break; + } + if(txt[1] != '\n') *txt = '?'; // help for wrong message length + switch(*txt){ + case '0': + can_accept_one(); + SEND("Accept only my ID @CAN"); + break; + case '@': + can_accept_any(); + SEND("Accept any ID @CAN"); + break; + case 'a': + showADCvals(); + break; + case 'b': + SEND("Jump to bootloader.\n"); + sendbuf(); + Jump2Boot(); + break; + case 'g': + SEND("Board address: "); + printuhex(refreshBRDaddr()); + SEND("\nCAN IN address (OUT=IN+1): "); + printuhex(getCANID()); + break; + case 'I': + spi_setup(); + SEND("SPI reinited, status="); printu(SPI_status); + break; + case 'j': + printmcut(); + break; + case 'k': + showUIvals(); + break; + case 'm': + monitCAN = !monitCAN; + SEND("CAN monitoring "); + if(monitCAN) SEND("ON"); + else SEND("OFF"); + break; + case 'R': + if(SPI_transmit(NULL, 4)){ + SEND("SPI error, status="); printu(SPI_status); + } else SEND("Wait data from SPI"); + break; + case 't': + usart_send_blocking("TEST test\n", 10); + SEND("Sent"); + break; + case 'T': + SEND("Tms="); printu(Tms); + SEND("\nCounter="); printu(TIM2->CNT); // 24 bit downcounting 2us period + break; + case 'z': + flashstorage_init(); + break; + default: // help + SEND( + "0 - accept only data for this device\n" + "@ - accept any IDs\n" + "a - get raw ADC values\n" + "b - switch to bootloader\n" + "g - get board address\n" + "I - reinit SPI\n" + "j - get MCU temperature\n" + "k - get U values\n" + "m - start/stop monitoring CAN bus\n" + "R - read 32 bits from SPI\n" + "s - send data over CAN: s ID [byte0..7]\n" + "S? - parameter setters\n" + "t - send test sequence over RS-485\n" + "T - print current time\n" + "U? - options for user configuration\n" + "z - reinit flash storage\n" + ); + break; + } +eof: + newline(); + sendbuf(); +} + +// print 32bit unsigned int +void printu(uint32_t val){ + char buf[11], *bufptr = &buf[10]; + *bufptr = 0; + if(!val){ + *(--bufptr) = '0'; + }else{ + while(val){ + *(--bufptr) = val % 10 + '0'; + val /= 10; + } + } + addtobuf(bufptr); +} + +// print 32bit unsigned int as hex +void printuhex(uint32_t val){ + addtobuf("0x"); + uint8_t *ptr = (uint8_t*)&val + 3; + int8_t i, j; + for(i = 0; i < 4; ++i, --ptr){ + for(j = 1; j > -1; --j){ + uint8_t half = (*ptr >> (4*j)) & 0x0f; + if(half < 10) bufputchar(half + '0'); + else bufputchar(half - 10 + 'a'); + } + } +} + +// THERE'S NO OVERFLOW PROTECTION IN NUMBER READ PROCEDURES! +// read decimal number +static char *getdec(char *buf, uint32_t *N){ + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '9'){ + break; + } + num *= 10; + num += c - '0'; + ++buf; + } + *N = num; + return buf; +} +// read hexadecimal number (without 0x prefix!) +static char *gethex(char *buf, uint32_t *N){ + uint32_t num = 0; + while(*buf){ + char c = *buf; + uint8_t M = 0; + if(c >= '0' && c <= '9'){ + M = '0'; + }else if(c >= 'A' && c <= 'F'){ + M = 'A' - 10; + }else if(c >= 'a' && c <= 'f'){ + M = 'a' - 10; + } + if(M){ + num <<= 4; + num += c - M; + }else{ + break; + } + ++buf; + } + *N = num; + return buf; +} +// read binary number (without 0b prefix!) +static char *getbin(char *buf, uint32_t *N){ + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '1'){ + break; + } + num <<= 1; + if(c == '1') num |= 1; + ++buf; + } + *N = num; + return buf; +} + +/** + * @brief getnum - read uint32_t from string (dec, hex or bin: 127, 0x7f, 0b1111111) + * @param buf - buffer with number and so on + * @param N - the number read + * @return pointer to first non-number symbol in buf (if it is == buf, there's no number) + */ +char *getnum(char *txt, uint32_t *N){ + txt = omit_spaces(txt); + if(*txt == '0'){ + if(txt[1] == 'x' || txt[1] == 'X') return gethex(txt+2, N); + if(txt[1] == 'b' || txt[1] == 'B') return getbin(txt+2, N); + } + return getdec(txt, N); +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/proto.h b/F0:F030,F042,F072/CANBUS_SSI/proto.h new file mode 100644 index 0000000..e3a78f0 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/proto.h @@ -0,0 +1,56 @@ +/* + * geany_encoding=koi8-r + * proto.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#pragma once +#ifndef __PROTO_H__ +#define __PROTO_H__ + +#include "hardware.h" + +// macro for static strings +#define SEND(str) do{addtobuf(str);}while(0) + +#ifdef EBUG +#define MSG(str) do{addtobuf(__FILE__ " (L" STR(__LINE__) "): " str);}while(0) +#else +#define MSG(str) +#endif + +#define newline() do{bufputchar('\n');}while(0) + +extern uint8_t monitCAN; + +char *omit_spaces(char *buf); + +void cmd_parser(char *buf, uint8_t isUSB); +void addtobuf(const char *txt); +void bufputchar(char ch); +void sendbuf(); +void printu(uint32_t val); +void printuhex(uint32_t val); +char *getnum(char *txt, uint32_t *N); + +#define TARGET_USB 1 +#define TARGET_USART 0 +void buftgt(uint8_t isUSB); + +#endif // __PROTO_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/spi.c b/F0:F030,F042,F072/CANBUS_SSI/spi.c new file mode 100644 index 0000000..9994b68 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/spi.c @@ -0,0 +1,129 @@ +/* + * This file is part of the Stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#include "hardware.h" +#include "proto.h" +#include "spi.h" + +#include // memcpy + +// buffers for DMA rx/tx +static uint8_t inbuff[SPIBUFSZ], outbuf[SPIBUFSZ], rxrdy = 0; +spiStatus SPI_status = SPI_NOTREADY; + +//SPI: PA5 - SCK, PA6 -MISO, PA7 - MOSI +void spi_setup(){ + /* (1) Select AF mode on PA5, PA6, PA7 */ + /* (2) AF0 for SPI1 signals */ + GPIOA->MODER = (GPIOA->MODER & ~(GPIO_MODER_MODER5 | GPIO_MODER_MODER6 | GPIO_MODER_MODER7)) | + (GPIO_MODER_MODER5_AF | GPIO_MODER_MODER6_AF | GPIO_MODER_MODER7_AF); /* (1) */ + GPIOA->AFR[0] = (GPIOA->AFR[0] & ~(GPIO_AFRL_AFRL5 | GPIO_AFRL_AFRL6 | GPIO_AFRL_AFRL7)); /* (2) */ + // Configure DMA SPI + /* Enable the peripheral clock DMA1 */ + RCC->AHBENR |= RCC_AHBENR_DMA1EN; + /* DMA1 Channel2 SPI1_RX config */ + /* (1) Peripheral address */ + /* (2) Memory address */ + /* (3) Data size */ + /* (4) Memory increment */ + /* Peripheral to memory */ + /* 8-bit transfer */ + DMA1_Channel2->CCR &= ~DMA_CCR_EN; + DMA1_Channel2->CPAR = (uint32_t)&(SPI1->DR); /* (1) */ + DMA1_Channel2->CMAR = (uint32_t)inbuff; /* (2) */ + DMA1_Channel2->CNDTR = SPIBUFSZ; /* (3) */ + DMA1_Channel2->CCR |= DMA_CCR_MINC | DMA_CCR_EN; /* (4) */ + /* DMA1 Channel3 SPI1_TX config */ + /* (5) Peripheral address */ + /* (6) Memory address */ + /* (7) Memory increment */ + /* Memory to peripheral*/ + /* 8-bit transfer */ + /* Transfer complete IT */ + DMA1_Channel3->CCR &= ~DMA_CCR_EN; + DMA1_Channel3->CPAR = (uint32_t)&(SPI1->DR); /* (5) */ + DMA1_Channel3->CMAR = (uint32_t)outbuf; /* (6) */ + DMA1_Channel3->CCR |= DMA_CCR_MINC | DMA_CCR_TCIE | DMA_CCR_DIR; /* (7) */ + /* Configure IT */ + /* (8) Set priority for DMA1_Channel2_3_IRQn */ + /* (9) Enable DMA1_Channel2_3_IRQn */ + NVIC_SetPriority(DMA1_Channel2_3_IRQn, 0); /* (8) */ + NVIC_EnableIRQ(DMA1_Channel2_3_IRQn); /* (9) */ + /* Enable the peripheral clock SPI1 */ + RCC->APB2ENR |= RCC_APB2ENR_SPI1EN; + /* Configure SPI1 in master */ + /* (1) Master selection, BR: Fpclk/128 CPOL and CPHA at zero (rising first edge) */ + /* (2) TX and RX with DMA, slave select output enabled, 8-bit Rx fifo */ + /* (3) Enable SPI1 */ + SPI1->CR1 = SPI_CR1_MSTR | SPI_CR1_BR_2 | SPI_CR1_BR_1 | SPI_CR1_SSM | SPI_CR1_SSI; /* (1) */ + SPI1->CR2 = SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN | SPI_CR2_FRXTH | SPI_CR2_DS_2 | SPI_CR2_DS_1 | SPI_CR2_DS_0; /* (2) */ + SPI1->CR1 |= SPI_CR1_SPE; /* (3) */ + SPI_status = SPI_READY; +} + +void dma1_channel2_3_isr(){ + if(DMA1->ISR & DMA_ISR_TCIF3){ + DMA1->IFCR |= DMA_IFCR_CTCIF3; + SPI_status = SPI_READY; + rxrdy = 1; +#ifdef EBUG + SEND("rxCNDTR="); printu(DMA1_Channel2->CNDTR); SEND(", txCNDTR="); printu(DMA1_Channel3->CNDTR); newline(); sendbuf(); +#endif + } +} + +/** + * @brief SPI_transmit - transmit data over SPI DMA + * @param buf - data to transmit + * @param len - its length + * @return 0 if all OK + */ +uint8_t SPI_transmit(const uint8_t *buf, uint8_t len){ + if(!len || len > SPIBUFSZ) return 1; // bad data format + if(SPI_status != SPI_READY) return 2; // spi not ready to transmit data + DMA1_Channel2->CCR &= ~DMA_CCR_EN; + DMA1_Channel3->CCR &=~ DMA_CCR_EN; + if(buf) memcpy(outbuf, buf, len); + else memset(outbuf, 0, len); + DMA1_Channel3->CNDTR = len; + DMA1_Channel2->CNDTR = len; + SPI_status = SPI_BUSY; +#ifdef EBUG + SEND("SPI tx "); printu(len); SEND(" bytes\n"); sendbuf(); +#endif + DMA1_Channel2->CCR |= DMA_CCR_EN; + DMA1_Channel3->CCR |= DMA_CCR_EN; + return 0; +} + +/** + * @brief SPI_getdata - get data received by DMA & reload receiver + * @param buf - buffer for data (with length maxlen) or NULL + * @param len - amount of bytes to copy into buf + * @return 1 if got data + */ +uint8_t SPI_getdata(uint8_t *buf, uint8_t len){ + if(SPI_status != SPI_READY || !rxrdy) return 0; + if(!buf || !len || len > SPIBUFSZ) return 0; + //SEND("def\n", 4); + uint8_t remain = DMA1_Channel2->CNDTR; + if(remain) return 0; + rxrdy = 0; + if(buf && len) memcpy(buf, inbuff, len); + return 1; +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/spi.h b/F0:F030,F042,F072/CANBUS_SSI/spi.h new file mode 100644 index 0000000..1637dd6 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/spi.h @@ -0,0 +1,39 @@ +/* + * This file is part of the Stepper project. + * Copyright 2020 Edward V. Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef SPI_H__ +#define SPI_H__ + +#include "stm32f0.h" + +#define SPIBUFSZ 64 + +typedef enum{ + SPI_NOTREADY, + SPI_READY, + SPI_BUSY +} spiStatus; + +extern spiStatus SPI_status; + +void spi_setup(); +uint8_t SPI_transmit(const uint8_t *buf, uint8_t len); +uint8_t SPI_getdata(uint8_t *buf, uint8_t len); + +#endif // SPI_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/usart.c b/F0:F030,F042,F072/CANBUS_SSI/usart.c new file mode 100644 index 0000000..65fef43 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usart.c @@ -0,0 +1,196 @@ +/* + * usart.c + * + * Copyright 2017 Edward V. Emelianoff + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#include "hardware.h" +#include "usart.h" + +#include +#include + +typedef enum{ + READING, + SENDING, + WAITING +} _485_state; +static _485_state st = READING; // RS-485 state: Rx, Tx, TX last byte + +// switch to Rx/Tx: +#define _485_Rx() do{RS485_RX(); st = READING; USARTX->CR1 = (USARTX->CR1 & ~USART_CR1_TE) | USART_CR1_RE;}while(0) +#define _485_Tx() do{RS485_TX(); st = SENDING; USARTX->CR1 = (USARTX->CR1 & ~USART_CR1_RE) | USART_CR1_TE;}while(0) + +static int datalen[2] = {0,0}; // received data line length (including '\n') + +volatile int + linerdy = 0, // received data ready + bufovr = 0 // input buffer overfull +; + +static volatile int + dlen = 0, // length of data (including '\n') in current buffer + txrdy = 1 // transmission done +; + +static int rbufno = 0; // current rbuf number +static char rbuf[UARTBUFSZ][2]; // receive buffers +static char *recvdata = NULL; + +/** + * return length of received data (without trailing zero) + */ +int usart_getline(char **line){ + if(!line) return 0; + if(bufovr){ + bufovr = 0; + linerdy = 0; + return 0; + } + *line = recvdata; + line[dlen] = 0; + linerdy = 0; + return dlen; +} + +/** + * @brief usart_send_blocking - blocking send of any message + * @param str - message to send + * @param len - its length + */ +void usart_send_blocking(const char *str, int len){ + uint32_t tmout = 160000; + while(!txrdy){ + IWDG->KR = IWDG_REFRESH; + if(--tmout == 0) return; + } + _485_Tx(); + bufovr = 0; + for(int i = 0; i < len; ++i){ + USARTX -> TDR = *str++; + while(!(USARTX->ISR & USART_ISR_TXE)){IWDG->KR = IWDG_REFRESH;} + } + // wait for transfer complete to switch into Rx + while(!(USARTX->ISR & USART_ISR_TC)){IWDG->KR = IWDG_REFRESH;} + _485_Rx(); +} + +void usart_setup(){ +#if USARTNUM == 2 + // setup pins: PA2 (Tx - AF1), PA15 (Rx - AF1) + // AF mode (AF1) + GPIOA->MODER = (GPIOA->MODER & ~(GPIO_MODER_MODER2|GPIO_MODER_MODER15))\ + | (GPIO_MODER_MODER2_AF | GPIO_MODER_MODER15_AF); + GPIOA->AFR[0] = (GPIOA->AFR[0] &~GPIO_AFRH_AFRH2) | 1 << (2 * 4); // PA2 + GPIOA->AFR[1] = (GPIOA->AFR[1] &~GPIO_AFRH_AFRH7) | 1 << (7 * 4); // PA15 + // setup usart2 + RCC->APB1ENR |= RCC_APB1ENR_USART2EN; // clock + // oversampling by16, 115200bps (fck=48mHz) + //USART2_BRR = 0x1a1; // 48000000 / 115200 + USART2->BRR = 480000 / 1152; + USART2->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE; // 1start,8data,nstop; enable Rx,Tx,USART + while(!(USART2->ISR & USART_ISR_TC)); // polling idle frame Transmission + USART2->ICR |= USART_ICR_TCCF; // clear TC flag + USART2->CR1 |= USART_CR1_RXNEIE; + NVIC_EnableIRQ(USART2_IRQn); +// USART1 of main board +#elif USARTNUM == 1 + // PA9 - Tx, PA10 - Rx (AF1) + GPIOA->MODER = (GPIOA->MODER & ~(GPIO_MODER_MODER9 | GPIO_MODER_MODER10))\ + | (GPIO_MODER_MODER9_AF | GPIO_MODER_MODER10_AF); + GPIOA->AFR[1] = (GPIOA->AFR[1] & ~(GPIO_AFRH_AFRH1 | GPIO_AFRH_AFRH2)) | + 1 << (1 * 4) | 1 << (2 * 4); // PA9, PA10 + // Tx CNDTR set @ each transmission due to data size + NVIC_SetPriority(USART1_IRQn, 0); + // setup usart1 + RCC->APB2ENR |= RCC_APB2ENR_USART1EN; + USART1->BRR = 480000 / 1152; + USART1->CR1 = USART_CR1_UE; // 1start,8data,nstop; enable USART + while(!(USART1->ISR & USART_ISR_TC)); // polling idle frame Transmission + USART1->ICR |= USART_ICR_TCCF; // clear TC flag + USART1->CR1 |= USART_CR1_RXNEIE; + NVIC_EnableIRQ(USART1_IRQn); +#else +#error "Not implemented" +#endif + _485_Rx(); // turn RX on (enable Rx, disable Tx) +} + +#if USARTNUM == 2 +void usart2_isr(){ +// USART1 +#elif USARTNUM == 1 +void usart1_isr(){ +#else +#error "Not implemented" +#endif + #ifdef CHECK_TMOUT + static uint32_t tmout = 0; + #endif + if(USARTX->ISR & USART_ISR_RXNE){ // RX not emty - receive next char + #ifdef CHECK_TMOUT + if(tmout && Tms >= tmout){ // set overflow flag + bufovr = 1; + datalen[rbufno] = 0; + } + tmout = Tms + TIMEOUT_MS; + if(!tmout) tmout = 1; // prevent 0 + #endif + // read RDR clears flag + uint8_t rb = USARTX->RDR; + if(datalen[rbufno] < UARTBUFSZ-1){ // put next char into buf + rbuf[rbufno][datalen[rbufno]++] = rb; + if(rb == '\n'){ // got newline - line ready + linerdy = 1; + dlen = datalen[rbufno]; + recvdata = rbuf[rbufno]; + // prepare other buffer + rbufno = !rbufno; + datalen[rbufno] = 0; + #ifdef CHECK_TMOUT + // clear timeout at line end + tmout = 0; + #endif + } + }else{ // buffer overfull + bufovr = 1; + datalen[rbufno] = 0; + #ifdef CHECK_TMOUT + tmout = 0; + #endif + } + } +} + + +/** + * @brief usart_proc - switch 485 to Rx when all data received + */ +void usart_proc(){ + switch(st){ + case SENDING: + if(txrdy) st = WAITING; + break; + case WAITING: + if(USARTX->ISR & USART_ISR_TC){ // last byte done -> Rx + _485_Rx(); + } + break; + default: + break; + } +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/usart.h b/F0:F030,F042,F072/CANBUS_SSI/usart.h new file mode 100644 index 0000000..21537b3 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usart.h @@ -0,0 +1,48 @@ +/* + * usart.h + * + * Copyright 2017 Edward V. Emelianoff + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + */ +#pragma once +#ifndef __USART_H__ +#define __USART_H__ + +// input and output buffers size +#define UARTBUFSZ (64) +// timeout between data bytes +#ifndef TIMEOUT_MS +#define TIMEOUT_MS (1500) +#endif + +typedef enum{ + ALL_OK, + LINE_BUSY, + STR_TOO_LONG +} TXstatus; + +#define usartrx() (linerdy) +#define usartovr() (bufovr) + +extern volatile int linerdy, bufovr; + +void usart_setup(); +int usart_getline(char **line); +void usart_send_blocking(const char *str, int len); +void usart_proc(); + +#endif // __USART_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/usb.c b/F0:F030,F042,F072/CANBUS_SSI/usb.c new file mode 100644 index 0000000..e8b4c52 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usb.c @@ -0,0 +1,181 @@ +/* + * geany_encoding=koi8-r + * usb.c - base functions for different USB types + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +#include "usb.h" +#include "usb_lib.h" +#include "usart.h" + +static volatile uint8_t tx_succesfull = 1; +static volatile uint8_t rxNE = 0; + +// interrupt IN handler (never used?) +static void EP1_Handler(){ + uint16_t epstatus = KEEP_DTOG(USB->EPnR[1]); + if(RX_FLAG(epstatus)) epstatus = (epstatus & ~USB_EPnR_STAT_TX) ^ USB_EPnR_STAT_RX; // set valid RX + else epstatus = epstatus & ~(USB_EPnR_STAT_TX|USB_EPnR_STAT_RX); + // clear CTR + epstatus = (epstatus & ~(USB_EPnR_CTR_RX|USB_EPnR_CTR_TX)); + USB->EPnR[1] = epstatus; +} + +// data IN/OUT handlers +static void transmit_Handler(){ // EP3IN + tx_succesfull = 1; + uint16_t epstatus = KEEP_DTOG_STAT(USB->EPnR[3]); + // clear CTR keep DTOGs & STATs + USB->EPnR[3] = (epstatus & ~(USB_EPnR_CTR_TX)); // clear TX ctr +} + +static void receive_Handler(){ // EP2OUT + rxNE = 1; + uint16_t epstatus = KEEP_DTOG_STAT(USB->EPnR[2]); + USB->EPnR[2] = (epstatus & ~(USB_EPnR_CTR_RX)); // clear RX ctr +} + +void USB_setup(){ + RCC->APB1ENR |= RCC_APB1ENR_CRSEN | RCC_APB1ENR_USBEN; // enable CRS (hsi48 sync) & USB + RCC->CFGR3 &= ~RCC_CFGR3_USBSW; // reset USB + RCC->CR2 |= RCC_CR2_HSI48ON; // turn ON HSI48 + uint32_t tmout = 16000000; + while(!(RCC->CR2 & RCC_CR2_HSI48RDY)){if(--tmout == 0) break;} + FLASH->ACR = FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY; + CRS->CFGR &= ~CRS_CFGR_SYNCSRC; + CRS->CFGR |= CRS_CFGR_SYNCSRC_1; // USB SOF selected as sync source + CRS->CR |= CRS_CR_AUTOTRIMEN; // enable auto trim + CRS->CR |= CRS_CR_CEN; // enable freq counter & block CRS->CFGR as read-only + RCC->CFGR |= RCC_CFGR_SW; + // allow RESET and CTRM interrupts + USB->CNTR = USB_CNTR_RESETM | USB_CNTR_WKUPM; + // clear flags + USB->ISTR = 0; + // and activate pullup + USB->BCDR |= USB_BCDR_DPPU; + NVIC_EnableIRQ(USB_IRQn); +} + + +static int usbwr(const uint8_t *buf, uint16_t l){ + uint32_t ctra = 1000000; + while(--ctra && tx_succesfull == 0){ + IWDG->KR = IWDG_REFRESH; + } + tx_succesfull = 0; + EP_Write(3, buf, l); + ctra = 1000000; + while(--ctra && tx_succesfull == 0){ + IWDG->KR = IWDG_REFRESH; + } + if(tx_succesfull == 0){usbON = 0; return 1;} // usb is OFF? + return 0; +} + +static uint8_t usbbuff[USB_TXBUFSZ-1]; // temporary buffer (63 - to prevent need of ZLP) +static uint8_t buflen = 0; // amount of symbols in usbbuff + +// send next up to 63 bytes of data in usbbuff +static void send_next(){ + if(!buflen || !tx_succesfull) return; + tx_succesfull = 0; + EP_Write(3, usbbuff, buflen); + buflen = 0; +} + +// unblocking sending - just fill a buffer +void USB_send(const uint8_t *buf, uint16_t len){ + if(!usbON || !len) return; + if(len > USB_TXBUFSZ-1 - buflen){ + usbwr(usbbuff, buflen); + buflen = 0; + } + if(len > USB_TXBUFSZ-1){ + USB_send_blk(buf, len); + return; + } + while(len--) usbbuff[buflen++] = *buf++; +} + +// send zero-terminated string +void USB_sendstr(const char *str){ + uint16_t l = 0; + const char *ptr = str; + while(*ptr++ && l < 512) ++l; + USB_send((uint8_t*)str, l); +} + +// blocking sending +void USB_send_blk(const uint8_t *buf, uint16_t len){ + if(!usbON || !len) return; // USB disconnected + if(buflen){ + usbwr(usbbuff, buflen); + buflen = 0; + } + int needzlp = 0; + while(len){ + if(len == USB_TXBUFSZ) needzlp = 1; + uint16_t s = (len > USB_TXBUFSZ) ? USB_TXBUFSZ : len; + if(usbwr(buf, s)) return; + len -= s; + buf += s; + } + if(needzlp){ + usbwr(NULL, 0); + } +} + +void usb_proc(){ + switch(USB_Dev.USB_Status){ + case USB_STATE_CONFIGURED: + // make new BULK endpoint + // Buffer have 1024 bytes, but last 256 we use for CAN bus (30.2 of RM: USB main features) + EP_Init(1, EP_TYPE_INTERRUPT, USB_EP1BUFSZ, 0, EP1_Handler); // IN1 - transmit + EP_Init(2, EP_TYPE_BULK, 0, USB_RXBUFSZ, receive_Handler); // OUT2 - receive data + EP_Init(3, EP_TYPE_BULK, USB_TXBUFSZ, 0, transmit_Handler); // IN3 - transmit data + USB_Dev.USB_Status = USB_STATE_CONNECTED; + break; + case USB_STATE_DEFAULT: + case USB_STATE_ADDRESSED: + if(usbON){ + usbON = 0; + } + break; + default: // USB_STATE_CONNECTED - send next data portion + if(!usbON) return; + send_next(); + } +} + +/** + * @brief USB_receive + * @param buf (i) - buffer[64] for received data + * @return amount of received bytes + */ +uint8_t USB_receive(uint8_t *buf){ + if(!usbON || !rxNE) return 0; + uint8_t sz = EP_Read(2, buf); + uint16_t epstatus = KEEP_DTOG(USB->EPnR[2]); + // keep stat_tx & set ACK rx + USB->EPnR[2] = (epstatus & ~(USB_EPnR_STAT_TX)) ^ USB_EPnR_STAT_RX; + rxNE = 0; + return sz; +} + diff --git a/F0:F030,F042,F072/CANBUS_SSI/usb.h b/F0:F030,F042,F072/CANBUS_SSI/usb.h new file mode 100644 index 0000000..c888a57 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usb.h @@ -0,0 +1,41 @@ +/* + * geany_encoding=koi8-r + * usb.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ +#pragma once +#ifndef __USB_H__ +#define __USB_H__ + +#include "hardware.h" + +#define BUFFSIZE (64) + +// send string with constant length +#define USND(str) do{USB_send((uint8_t*)str, sizeof(str)-1);}while(0) + +void USB_setup(); +void usb_proc(); +void USB_send(const uint8_t *buf, uint16_t len); +void USB_sendstr(const char *str); +void USB_send_blk(const uint8_t *buf, uint16_t len); +uint8_t USB_receive(uint8_t *buf); + +#endif // __USB_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/usb_defs.h b/F0:F030,F042,F072/CANBUS_SSI/usb_defs.h new file mode 100644 index 0000000..f7e1339 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usb_defs.h @@ -0,0 +1,104 @@ +/* + * geany_encoding=koi8-r + * usb_defs.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +#pragma once +#ifndef __USB_DEFS_H__ +#define __USB_DEFS_H__ + +#include + +// max endpoints number +#define STM32ENDPOINTS 8 +/** + * Buffers size definition + **/ +// !!! when working with CAN bus change USB_BTABLE_SIZE to 768 !!! +#define USB_BTABLE_SIZE 768 +// for USB FS EP0 buffers are from 8 to 64 bytes long (64 for PL2303) +#define USB_EP0_BUFSZ 64 +// USB transmit buffer size (64 for PL2303) +#define USB_TXBUFSZ 64 +// USB receive buffer size (64 for PL2303) +#define USB_RXBUFSZ 64 +// EP1 - interrupt - buffer size +#define USB_EP1BUFSZ 8 + +#define USB_BTABLE_BASE 0x40006000 + +#ifdef USB_BTABLE +#undef USB_BTABLE +#endif +#define USB_BTABLE ((USB_BtableDef *)(USB_BTABLE_BASE)) +#define USB_ISTR_EPID 0x0000000F +#define USB_FNR_LSOF_0 0x00000800 +#define USB_FNR_lSOF_1 0x00001000 +#define USB_LPMCSR_BESL_0 0x00000010 +#define USB_LPMCSR_BESL_1 0x00000020 +#define USB_LPMCSR_BESL_2 0x00000040 +#define USB_LPMCSR_BESL_3 0x00000080 +#define USB_EPnR_CTR_RX 0x00008000 +#define USB_EPnR_DTOG_RX 0x00004000 +#define USB_EPnR_STAT_RX 0x00003000 +#define USB_EPnR_STAT_RX_0 0x00001000 +#define USB_EPnR_STAT_RX_1 0x00002000 +#define USB_EPnR_SETUP 0x00000800 +#define USB_EPnR_EP_TYPE 0x00000600 +#define USB_EPnR_EP_TYPE_0 0x00000200 +#define USB_EPnR_EP_TYPE_1 0x00000400 +#define USB_EPnR_EP_KIND 0x00000100 +#define USB_EPnR_CTR_TX 0x00000080 +#define USB_EPnR_DTOG_TX 0x00000040 +#define USB_EPnR_STAT_TX 0x00000030 +#define USB_EPnR_STAT_TX_0 0x00000010 +#define USB_EPnR_STAT_TX_1 0x00000020 +#define USB_EPnR_EA 0x0000000F +#define USB_COUNTn_RX_BLSIZE 0x00008000 +#define USB_COUNTn_NUM_BLOCK 0x00007C00 +#define USB_COUNTn_RX 0x0000003F + +#define USB_TypeDef USB_TypeDef_custom + +typedef struct{ + __IO uint32_t EPnR[STM32ENDPOINTS]; + __IO uint32_t RESERVED[STM32ENDPOINTS]; + __IO uint32_t CNTR; + __IO uint32_t ISTR; + __IO uint32_t FNR; + __IO uint32_t DADDR; + __IO uint32_t BTABLE; + __IO uint32_t LPMCSR; + __IO uint32_t BCDR; +} USB_TypeDef; + +typedef struct{ + __IO uint16_t USB_ADDR_TX; + __IO uint16_t USB_COUNT_TX; + __IO uint16_t USB_ADDR_RX; + __IO uint16_t USB_COUNT_RX; +} USB_EPDATA_TypeDef; + +typedef struct{ + __IO USB_EPDATA_TypeDef EP[STM32ENDPOINTS]; +} USB_BtableDef; + +#endif // __USB_DEFS_H__ diff --git a/F0:F030,F042,F072/CANBUS_SSI/usb_lib.c b/F0:F030,F042,F072/CANBUS_SSI/usb_lib.c new file mode 100644 index 0000000..9449ca9 --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usb_lib.c @@ -0,0 +1,473 @@ +/* + * geany_encoding=koi8-r + * usb_lib.c + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +#include +#include "usb_lib.h" + +ep_t endpoints[STM32ENDPOINTS]; + +usb_dev_t USB_Dev; +uint8_t usbON = 0; +static usb_LineCoding lineCoding = {115200, 0, 0, 8}; +static config_pack_t setup_packet; +static uint8_t ep0databuf[EP0DATABUF_SIZE]; +static uint8_t ep0dbuflen = 0; + +usb_LineCoding getLineCoding(){return lineCoding;} + +// definition of parts common for USB_DeviceDescriptor & USB_DeviceQualifierDescriptor +#define bcdUSB_L 0x10 +#define bcdUSB_H 0x01 +#define bDeviceClass 0 +#define bDeviceSubClass 0 +#define bDeviceProtocol 0 +#define bNumConfigurations 1 + +static const uint8_t USB_DeviceDescriptor[] = { + 18, // bLength + 0x01, // bDescriptorType - Device descriptor + bcdUSB_L, // bcdUSB_L - 1.10 + bcdUSB_H, // bcdUSB_H + bDeviceClass, // bDeviceClass - USB_COMM + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0_BUFSZ, // bMaxPacketSize + 0x7b, // idVendor_L PL2303: VID=0x067b, PID=0x2303 + 0x06, // idVendor_H + 0x03, // idProduct_L + 0x23, // idProduct_H + 0x00, // bcdDevice_Ver_L + 0x03, // bcdDevice_Ver_H + 0x01, // iManufacturer + 0x02, // iProduct + 0x00, // iSerialNumber + bNumConfigurations // bNumConfigurations +}; + +static const uint8_t USB_DeviceQualifierDescriptor[] = { + 10, //bLength + 0x06, // bDescriptorType - Device qualifier + bcdUSB_L, // bcdUSB_L + bcdUSB_H, // bcdUSB_H + bDeviceClass, // bDeviceClass + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0_BUFSZ, // bMaxPacketSize0 + bNumConfigurations, // bNumConfigurations + 0x00 // Reserved +}; + +static const uint8_t USB_ConfigDescriptor[] = { + /*Configuration Descriptor*/ + 0x09, /* bLength: Configuration Descriptor size */ + 0x02, /* bDescriptorType: Configuration */ + 39, /* wTotalLength:no of returned bytes */ + 0x00, + 0x01, /* bNumInterfaces: 1 interface */ + 0x01, /* bConfigurationValue: Configuration value */ + 0x00, /* iConfiguration: Index of string descriptor describing the configuration */ + 0xa0, /* bmAttributes - Bus powered, Remote wakeup */ + 0x32, /* MaxPower 100 mA */ + + /*---------------------------------------------------------------------------*/ + + /*Interface Descriptor */ + 0x09, /* bLength: Interface Descriptor size */ + 0x04, /* bDescriptorType: Interface */ + 0x00, /* bInterfaceNumber: Number of Interface */ + 0x00, /* bAlternateSetting: Alternate setting */ + 0x03, /* bNumEndpoints: 3 endpoints used */ + 0xff, /* bInterfaceClass */ + 0x00, /* bInterfaceSubClass */ + 0x00, /* bInterfaceProtocol */ + 0x00, /* iInterface: */ +/////////////////////////////////////////////////// + /*Endpoint 1 Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + 0x05, /* bDescriptorType: Endpoint */ + 0x81, /* bEndpointAddress IN1 */ + 0x03, /* bmAttributes: Interrupt */ + 0x0a, /* wMaxPacketSize LO: */ + 0x00, /* wMaxPacketSize HI: */ + 0x01, /* bInterval: */ + + /*Endpoint OUT2 Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + 0x05, /* bDescriptorType: Endpoint */ + 0x02, /* bEndpointAddress: OUT2 */ + 0x02, /* bmAttributes: Bulk */ + (USB_RXBUFSZ & 0xff), /* wMaxPacketSize: 64 */ + (USB_RXBUFSZ >> 8), + 0x00, /* bInterval: ignore for Bulk transfer */ + + /*Endpoint IN3 Descriptor*/ + 0x07, /* bLength: Endpoint Descriptor size */ + 0x05, /* bDescriptorType: Endpoint */ + 0x83, /* bEndpointAddress IN3 */ + 0x02, /* bmAttributes: Bulk */ + (USB_TXBUFSZ & 0xff), /* wMaxPacketSize: 64 */ + (USB_TXBUFSZ >> 8), + 0x00, /* bInterval: ignore for Bulk transfer */ +}; + +_USB_LANG_ID_(USB_StringLangDescriptor, LANG_US); +// these descriptors are not used in PL2303 emulator! +_USB_STRING_(USB_StringSerialDescriptor, u"0"); +_USB_STRING_(USB_StringManufacturingDescriptor, u"Prolific Technology Inc."); +_USB_STRING_(USB_StringProdDescriptor, u"USB-Serial Controller"); + +/* + * default handlers + */ +// SET_LINE_CODING +void WEAK linecoding_handler(usb_LineCoding __attribute__((unused)) *lc){ +} + +// SET_CONTROL_LINE_STATE +void WEAK clstate_handler(uint16_t __attribute__((unused)) val){ +} + +// SEND_BREAK +void WEAK break_handler(){ +} + +// handler of vendor requests +void WEAK vendor_handler(config_pack_t *packet){ + if(packet->bmRequestType & 0x80){ // read + uint8_t c; + switch(packet->wValue){ + case 0x8484: + c = 2; + break; + case 0x0080: + c = 1; + break; + case 0x8686: + c = 0xaa; + break; + default: + c = 0; + } + EP_WriteIRQ(0, &c, 1); + }else{ // write ZLP + EP_WriteIRQ(0, (uint8_t *)0, 0); + } +} + +static void wr0(const uint8_t *buf, uint16_t size){ + if(setup_packet.wLength < size) size = setup_packet.wLength; // shortened request + if(size < endpoints[0].txbufsz){ + EP_WriteIRQ(0, buf, size); + return; + } + while(size){ + uint16_t l = size; + if(l > endpoints[0].txbufsz) l = endpoints[0].txbufsz; + EP_WriteIRQ(0, buf, l); + buf += l; + size -= l; + uint8_t needzlp = (l == endpoints[0].txbufsz) ? 1 : 0; + if(size || needzlp){ // send last data buffer + uint16_t status = KEEP_DTOG(USB->EPnR[0]); + // keep DTOGs, clear CTR_RX,TX, set TX VALID, leave stat_Rx + USB->EPnR[0] = (status & ~(USB_EPnR_CTR_RX|USB_EPnR_CTR_TX|USB_EPnR_STAT_RX)) + ^ USB_EPnR_STAT_TX; + uint32_t ctr = 1000000; + while(--ctr && (USB->ISTR & USB_ISTR_CTR) == 0){IWDG->KR = IWDG_REFRESH;}; + if((USB->ISTR & USB_ISTR_CTR) == 0){ + return; + } + if(needzlp) EP_WriteIRQ(0, (uint8_t*)0, 0); + } + } +} + +static inline void get_descriptor(){ + switch(setup_packet.wValue){ + case DEVICE_DESCRIPTOR: + wr0(USB_DeviceDescriptor, sizeof(USB_DeviceDescriptor)); + break; + case CONFIGURATION_DESCRIPTOR: + wr0(USB_ConfigDescriptor, sizeof(USB_ConfigDescriptor)); + break; + case STRING_LANG_DESCRIPTOR: + wr0((const uint8_t *)&USB_StringLangDescriptor, STRING_LANG_DESCRIPTOR_SIZE_BYTE); + break; + case STRING_MAN_DESCRIPTOR: + wr0((const uint8_t *)&USB_StringManufacturingDescriptor, USB_StringManufacturingDescriptor.bLength); + break; + case STRING_PROD_DESCRIPTOR: + wr0((const uint8_t *)&USB_StringProdDescriptor, USB_StringProdDescriptor.bLength); + break; + case STRING_SN_DESCRIPTOR: + wr0((const uint8_t *)&USB_StringSerialDescriptor, USB_StringSerialDescriptor.bLength); + break; + case DEVICE_QUALIFIER_DESCRIPTOR: + wr0(USB_DeviceQualifierDescriptor, USB_DeviceQualifierDescriptor[0]); + break; + default: + break; + } +} + +static uint8_t configuration = 0; // reply for GET_CONFIGURATION (==1 if configured) +static inline void std_d2h_req(){ + uint16_t status = 0; // bus powered + switch(setup_packet.bRequest){ + case GET_DESCRIPTOR: + get_descriptor(); + break; + case GET_STATUS: + EP_WriteIRQ(0, (uint8_t *)&status, 2); // send status: Bus Powered + break; + case GET_CONFIGURATION: + EP_WriteIRQ(0, &configuration, 1); + break; + default: + break; + } +} + +static inline void std_h2d_req(){ + switch(setup_packet.bRequest){ + case SET_ADDRESS: + // new address will be assigned later - after acknowlegement or request to host + USB_Dev.USB_Addr = setup_packet.wValue; + break; + case SET_CONFIGURATION: + // Now device configured + USB_Dev.USB_Status = USB_STATE_CONFIGURED; + configuration = setup_packet.wValue; + break; + default: + break; + } +} + +/* +bmRequestType: 76543210 +7 direction: 0 - host->device, 1 - device->host +65 type: 0 - standard, 1 - class, 2 - vendor +4..0 getter: 0 - device, 1 - interface, 2 - endpoint, 3 - other +*/ +/** + * Endpoint0 (control) handler + */ +static void EP0_Handler(){ + uint16_t epstatus = USB->EPnR[0]; // EP0R on input -> return this value after modifications + uint8_t reqtype = setup_packet.bmRequestType & 0x7f; + uint8_t dev2host = (setup_packet.bmRequestType & 0x80) ? 1 : 0; + int rxflag = RX_FLAG(epstatus); + if(rxflag && SETUP_FLAG(epstatus)){ + switch(reqtype){ + case STANDARD_DEVICE_REQUEST_TYPE: // standard device request + if(dev2host){ + std_d2h_req(); + }else{ + std_h2d_req(); + EP_WriteIRQ(0, (uint8_t *)0, 0); + } + break; + case STANDARD_ENDPOINT_REQUEST_TYPE: // standard endpoint request + if(setup_packet.bRequest == CLEAR_FEATURE){ + EP_WriteIRQ(0, (uint8_t *)0, 0); + } + break; + case VENDOR_REQUEST_TYPE: + vendor_handler(&setup_packet); + break; + case CONTROL_REQUEST_TYPE: + switch(setup_packet.bRequest){ + case GET_LINE_CODING: + EP_WriteIRQ(0, (uint8_t*)&lineCoding, sizeof(lineCoding)); + break; + case SET_LINE_CODING: // omit this for next stage, when data will come + break; + case SET_CONTROL_LINE_STATE: + usbON = 1; + clstate_handler(setup_packet.wValue); + break; + case SEND_BREAK: + usbON = 0; + break_handler(); + break; + default: + break; + } + if(setup_packet.bRequest != GET_LINE_CODING) EP_WriteIRQ(0, (uint8_t *)0, 0); // write acknowledgement + break; + default: + EP_WriteIRQ(0, (uint8_t *)0, 0); + } + }else if(rxflag){ // got data over EP0 or host acknowlegement + if(endpoints[0].rx_cnt){ + if(setup_packet.bRequest == SET_LINE_CODING){ + linecoding_handler((usb_LineCoding*)ep0databuf); + } + } + } else if(TX_FLAG(epstatus)){ // package transmitted + // now we can change address after enumeration + if ((USB->DADDR & USB_DADDR_ADD) != USB_Dev.USB_Addr){ + USB->DADDR = USB_DADDR_EF | USB_Dev.USB_Addr; + // change state to ADRESSED + USB_Dev.USB_Status = USB_STATE_ADDRESSED; + } + } + epstatus = KEEP_DTOG(USB->EPnR[0]); + if(rxflag) epstatus ^= USB_EPnR_STAT_TX; // start ZLP/data transmission + else epstatus &= ~USB_EPnR_STAT_TX; // or leave unchanged + // keep DTOGs, clear CTR_RX,TX, set RX VALID + USB->EPnR[0] = (epstatus & ~(USB_EPnR_CTR_RX|USB_EPnR_CTR_TX)) ^ USB_EPnR_STAT_RX; +} + +static uint16_t lastaddr = LASTADDR_DEFAULT; +/** + * Endpoint initialisation + * !!! when working with CAN bus change USB_BTABLE_SIZE to 768 !!! + * @param number - EP num (0...7) + * @param type - EP type (EP_TYPE_BULK, EP_TYPE_CONTROL, EP_TYPE_ISO, EP_TYPE_INTERRUPT) + * @param txsz - transmission buffer size @ USB/CAN buffer + * @param rxsz - reception buffer size @ USB/CAN buffer + * @param uint16_t (*func)(ep_t *ep) - EP handler function + * @return 0 if all OK + */ +int EP_Init(uint8_t number, uint8_t type, uint16_t txsz, uint16_t rxsz, void (*func)()){ + if(number >= STM32ENDPOINTS) return 4; // out of configured amount + if(txsz > USB_BTABLE_SIZE || rxsz > USB_BTABLE_SIZE) return 1; // buffer too large + if(lastaddr + txsz + rxsz >= USB_BTABLE_SIZE) return 2; // out of btable + USB->EPnR[number] = (type << 9) | (number & USB_EPnR_EA); + USB->EPnR[number] ^= USB_EPnR_STAT_RX | USB_EPnR_STAT_TX_1; + if(rxsz & 1 || rxsz > 512) return 3; // wrong rx buffer size + uint16_t countrx = 0; + if(rxsz < 64) countrx = rxsz / 2; + else{ + if(rxsz & 0x1f) return 3; // should be multiple of 32 + countrx = 31 + rxsz / 32; + } + USB_BTABLE->EP[number].USB_ADDR_TX = lastaddr; + endpoints[number].tx_buf = (uint16_t *)(USB_BTABLE_BASE + lastaddr); + endpoints[number].txbufsz = txsz; + lastaddr += txsz; + USB_BTABLE->EP[number].USB_COUNT_TX = 0; + USB_BTABLE->EP[number].USB_ADDR_RX = lastaddr; + endpoints[number].rx_buf = (uint8_t *)(USB_BTABLE_BASE + lastaddr); + lastaddr += rxsz; + // buffer size: Table127 of RM + USB_BTABLE->EP[number].USB_COUNT_RX = countrx << 10; + endpoints[number].func = func; + return 0; +} + +// standard IRQ handler +void usb_isr(){ + if (USB->ISTR & USB_ISTR_RESET){ + // Reinit registers + USB->CNTR = USB_CNTR_RESETM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM; + USB->ISTR = 0; + // Endpoint 0 - CONTROL + // ON USB LS size of EP0 may be 8 bytes, but on FS it should be 64 bytes! + lastaddr = LASTADDR_DEFAULT; // roll back to beginning of buffer + EP_Init(0, EP_TYPE_CONTROL, USB_EP0_BUFSZ, USB_EP0_BUFSZ, EP0_Handler); + // clear address, leave only enable bit + USB->DADDR = USB_DADDR_EF; + // state is default - wait for enumeration + USB_Dev.USB_Status = USB_STATE_DEFAULT; + } + if(USB->ISTR & USB_ISTR_CTR){ + // EP number + uint8_t n = USB->ISTR & USB_ISTR_EPID; + // copy status register + uint16_t epstatus = USB->EPnR[n]; + // copy received bytes amount + endpoints[n].rx_cnt = USB_BTABLE->EP[n].USB_COUNT_RX & 0x3FF; // low 10 bits is counter + // check direction + if(USB->ISTR & USB_ISTR_DIR){ // OUT interrupt - receive data, CTR_RX==1 (if CTR_TX == 1 - two pending transactions: receive following by transmit) + if(n == 0){ // control endpoint + if(epstatus & USB_EPnR_SETUP){ // setup packet -> copy data to conf_pack + EP_Read(0, (uint8_t*)&setup_packet); + ep0dbuflen = 0; + // interrupt handler will be called later + }else if(epstatus & USB_EPnR_CTR_RX){ // data packet -> push received data to ep0databuf + ep0dbuflen = endpoints[0].rx_cnt; + EP_Read(0, (uint8_t*)&ep0databuf); + } + } + } + // call EP handler + if(endpoints[n].func) endpoints[n].func(endpoints[n]); + } + if(USB->ISTR & USB_ISTR_SUSP){ // suspend -> still no connection, may sleep + usbON = 0; + USB->CNTR |= USB_CNTR_FSUSP | USB_CNTR_LPMODE; + USB->ISTR = ~USB_ISTR_SUSP; + } + if(USB->ISTR & USB_ISTR_WKUP){ // wakeup + USB->CNTR &= ~(USB_CNTR_FSUSP | USB_CNTR_LPMODE); // clear suspend flags + USB->ISTR = ~USB_ISTR_WKUP; + } +} + +/** + * Write data to EP buffer (called from IRQ handler) + * @param number - EP number + * @param *buf - array with data + * @param size - its size + */ +void EP_WriteIRQ(uint8_t number, const uint8_t *buf, uint16_t size){ + uint8_t i; + if(size > USB_TXBUFSZ) size = USB_TXBUFSZ; + uint16_t N2 = (size + 1) >> 1; + // the buffer is 16-bit, so we should copy data as it would be uint16_t + uint16_t *buf16 = (uint16_t *)buf; + for (i = 0; i < N2; i++){ + endpoints[number].tx_buf[i] = buf16[i]; + } + USB_BTABLE->EP[number].USB_COUNT_TX = size; +} + +/** + * Write data to EP buffer (called outside IRQ handler) + * @param number - EP number + * @param *buf - array with data + * @param size - its size + */ +void EP_Write(uint8_t number, const uint8_t *buf, uint16_t size){ + EP_WriteIRQ(number, buf, size); + uint16_t status = KEEP_DTOG(USB->EPnR[number]); + // keep DTOGs, clear CTR_TX & set TX VALID to start transmission + USB->EPnR[number] = (status & ~(USB_EPnR_CTR_TX)) ^ USB_EPnR_STAT_TX; +} + +/* + * Copy data from EP buffer into user buffer area + * @param *buf - user array for data + * @return amount of data read + */ +int EP_Read(uint8_t number, uint8_t *buf){ + int n = endpoints[number].rx_cnt; + if(n){ + for(int i = 0; i < n; ++i) + buf[i] = endpoints[number].rx_buf[i]; + } + return n; +} diff --git a/F0:F030,F042,F072/CANBUS_SSI/usb_lib.h b/F0:F030,F042,F072/CANBUS_SSI/usb_lib.h new file mode 100644 index 0000000..92eea1f --- /dev/null +++ b/F0:F030,F042,F072/CANBUS_SSI/usb_lib.h @@ -0,0 +1,189 @@ +/* + * geany_encoding=koi8-r + * usb_lib.h + * + * Copyright 2018 Edward V. Emelianov + * + * 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 2 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, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + * MA 02110-1301, USA. + * + */ + +#pragma once +#ifndef __USB_LIB_H__ +#define __USB_LIB_H__ + +#include +#include "usb_defs.h" + +#define EP0DATABUF_SIZE (64) +#define LASTADDR_DEFAULT (STM32ENDPOINTS * 8) + +// bmRequestType & 0x7f +#define STANDARD_DEVICE_REQUEST_TYPE 0 +#define STANDARD_ENDPOINT_REQUEST_TYPE 2 +#define VENDOR_REQUEST_TYPE 0x40 +#define CONTROL_REQUEST_TYPE 0x21 +// bRequest, standard; for bmRequestType == 0x80 +#define GET_STATUS 0x00 +#define GET_DESCRIPTOR 0x06 +#define GET_CONFIGURATION 0x08 +// for bmRequestType == 0 +#define CLEAR_FEATURE 0x01 +#define SET_FEATURE 0x03 // unused +#define SET_ADDRESS 0x05 +#define SET_DESCRIPTOR 0x07 // unused +#define SET_CONFIGURATION 0x09 +// for bmRequestType == 0x81, 1 or 0xB2 +#define GET_INTERFACE 0x0A // unused +#define SET_INTERFACE 0x0B // unused +#define SYNC_FRAME 0x0C // unused +#define VENDOR_REQUEST 0x01 // unused + +// Class-Specific Control Requests +#define SEND_ENCAPSULATED_COMMAND 0x00 // unused +#define GET_ENCAPSULATED_RESPONSE 0x01 // unused +#define SET_COMM_FEATURE 0x02 // unused +#define GET_COMM_FEATURE 0x03 // unused +#define CLEAR_COMM_FEATURE 0x04 // unused +#define SET_LINE_CODING 0x20 +#define GET_LINE_CODING 0x21 +#define SET_CONTROL_LINE_STATE 0x22 +#define SEND_BREAK 0x23 + +// control line states +#define CONTROL_DTR 0x01 +#define CONTROL_RTS 0x02 + +// wValue +#define DEVICE_DESCRIPTOR 0x100 +#define CONFIGURATION_DESCRIPTOR 0x200 +#define STRING_LANG_DESCRIPTOR 0x300 +#define STRING_MAN_DESCRIPTOR 0x301 +#define STRING_PROD_DESCRIPTOR 0x302 +#define STRING_SN_DESCRIPTOR 0x303 +#define DEVICE_QUALIFIER_DESCRIPTOR 0x600 + +#define RX_FLAG(epstat) (epstat & USB_EPnR_CTR_RX) +#define TX_FLAG(epstat) (epstat & USB_EPnR_CTR_TX) +#define SETUP_FLAG(epstat) (epstat & USB_EPnR_SETUP) + +// EPnR bits manipulation +#define KEEP_DTOG_STAT(EPnR) (EPnR & ~(USB_EPnR_STAT_RX|USB_EPnR_STAT_TX|USB_EPnR_DTOG_RX|USB_EPnR_DTOG_TX)) +#define KEEP_DTOG(EPnR) (EPnR & ~(USB_EPnR_DTOG_RX|USB_EPnR_DTOG_TX)) + +// USB state: uninitialized, addressed, ready for use, client connected +typedef enum{ + USB_STATE_DEFAULT, + USB_STATE_ADDRESSED, + USB_STATE_CONFIGURED, + USB_STATE_CONNECTED +} USB_state; + +// EP types +#define EP_TYPE_BULK 0x00 +#define EP_TYPE_CONTROL 0x01 +#define EP_TYPE_ISO 0x02 +#define EP_TYPE_INTERRUPT 0x03 + +#define LANG_US (uint16_t)0x0409 + +#define _USB_STRING_(name, str) \ +static const struct name \ +{ \ + uint8_t bLength; \ + uint8_t bDescriptorType; \ + uint16_t bString[(sizeof(str) - 2) / 2]; \ + \ +} \ +name = {sizeof(name), 0x03, str} + +#define _USB_LANG_ID_(name, lng_id) \ + \ +static const struct name \ +{ \ + uint8_t bLength; \ + uint8_t bDescriptorType; \ + uint16_t bString; \ + \ +} \ +name = {0x04, 0x03, lng_id} +#define STRING_LANG_DESCRIPTOR_SIZE_BYTE (4) + +// EP0 configuration packet +typedef struct { + uint8_t bmRequestType; + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} config_pack_t; + +// endpoints state +typedef struct __ep_t{ + uint16_t *tx_buf; // transmission buffer address + uint16_t txbufsz; // transmission buffer size + uint8_t *rx_buf; // reception buffer address + void (*func)(); // endpoint action function + uint16_t rx_cnt; // received data counter +} ep_t; + +// USB status & its address +typedef struct { + uint8_t USB_Status; + uint16_t USB_Addr; +}usb_dev_t; + +typedef struct { + uint32_t dwDTERate; + uint8_t bCharFormat; + #define USB_CDC_1_STOP_BITS 0 + #define USB_CDC_1_5_STOP_BITS 1 + #define USB_CDC_2_STOP_BITS 2 + uint8_t bParityType; + #define USB_CDC_NO_PARITY 0 + #define USB_CDC_ODD_PARITY 1 + #define USB_CDC_EVEN_PARITY 2 + #define USB_CDC_MARK_PARITY 3 + #define USB_CDC_SPACE_PARITY 4 + uint8_t bDataBits; +} __attribute__ ((packed)) usb_LineCoding; + +typedef struct { + uint8_t bmRequestType; + uint8_t bNotificationType; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} __attribute__ ((packed)) usb_cdc_notification; + +extern ep_t endpoints[]; +extern usb_dev_t USB_Dev; +extern uint8_t usbON; + +void USB_Init(); +uint8_t USB_GetState(); +int EP_Init(uint8_t number, uint8_t type, uint16_t txsz, uint16_t rxsz, void (*func)()); +void EP_WriteIRQ(uint8_t number, const uint8_t *buf, uint16_t size); +void EP_Write(uint8_t number, const uint8_t *buf, uint16_t size); +int EP_Read(uint8_t number, uint8_t *buf); +usb_LineCoding getLineCoding(); + +void linecoding_handler(usb_LineCoding *lc); +void clstate_handler(uint16_t val); +void break_handler(); +void vendor_handler(config_pack_t *packet); + +#endif // __USB_LIB_H__