From f3c3bf451b63eaa7d84b8b4946168977a42be929 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 19 Sep 2025 23:30:21 +0300 Subject: [PATCH] start --- F3:F303/MLX90640/Makefile | 10 + F3:F303/MLX90640/hardware.c | 34 +++ F3:F303/MLX90640/hardware.h | 31 ++ F3:F303/MLX90640/i2c.c | 380 +++++++++++++++++++++++++ F3:F303/MLX90640/i2c.h | 57 ++++ F3:F303/MLX90640/main.c | 68 +++++ F3:F303/MLX90640/mlx90640.bin | Bin 0 -> 7284 bytes F3:F303/MLX90640/mlx90640.cflags | 1 + F3:F303/MLX90640/mlx90640.config | 7 + F3:F303/MLX90640/mlx90640.creator | 1 + F3:F303/MLX90640/mlx90640.creator.user | 215 ++++++++++++++ F3:F303/MLX90640/mlx90640.cxxflags | 1 + F3:F303/MLX90640/mlx90640.files | 20 ++ F3:F303/MLX90640/mlx90640.includes | 6 + F3:F303/MLX90640/openocd.cfg | 119 ++++++++ F3:F303/MLX90640/proto.c | 156 ++++++++++ F3:F303/MLX90640/proto.h | 22 ++ F3:F303/MLX90640/ringbuffer.c | 163 +++++++++++ F3:F303/MLX90640/ringbuffer.h | 41 +++ F3:F303/MLX90640/strfunc.c | 306 ++++++++++++++++++++ F3:F303/MLX90640/strfunc.h | 32 +++ F3:F303/MLX90640/usb_descr.c | 210 ++++++++++++++ F3:F303/MLX90640/usb_descr.h | 62 ++++ F3:F303/MLX90640/usb_dev.c | 240 ++++++++++++++++ F3:F303/MLX90640/usb_dev.h | 58 ++++ F3:F303/MLX90640/usb_lib.c | 368 ++++++++++++++++++++++++ F3:F303/MLX90640/usb_lib.h | 328 +++++++++++++++++++++ F3:F303/MLX90640/version.inc | 2 + 28 files changed, 2938 insertions(+) create mode 100644 F3:F303/MLX90640/Makefile create mode 100644 F3:F303/MLX90640/hardware.c create mode 100644 F3:F303/MLX90640/hardware.h create mode 100644 F3:F303/MLX90640/i2c.c create mode 100644 F3:F303/MLX90640/i2c.h create mode 100644 F3:F303/MLX90640/main.c create mode 100755 F3:F303/MLX90640/mlx90640.bin create mode 100644 F3:F303/MLX90640/mlx90640.cflags create mode 100644 F3:F303/MLX90640/mlx90640.config create mode 100644 F3:F303/MLX90640/mlx90640.creator create mode 100644 F3:F303/MLX90640/mlx90640.creator.user create mode 100644 F3:F303/MLX90640/mlx90640.cxxflags create mode 100644 F3:F303/MLX90640/mlx90640.files create mode 100644 F3:F303/MLX90640/mlx90640.includes create mode 100644 F3:F303/MLX90640/openocd.cfg create mode 100644 F3:F303/MLX90640/proto.c create mode 100644 F3:F303/MLX90640/proto.h create mode 100644 F3:F303/MLX90640/ringbuffer.c create mode 100644 F3:F303/MLX90640/ringbuffer.h create mode 100644 F3:F303/MLX90640/strfunc.c create mode 100644 F3:F303/MLX90640/strfunc.h create mode 100644 F3:F303/MLX90640/usb_descr.c create mode 100644 F3:F303/MLX90640/usb_descr.h create mode 100644 F3:F303/MLX90640/usb_dev.c create mode 100644 F3:F303/MLX90640/usb_dev.h create mode 100644 F3:F303/MLX90640/usb_lib.c create mode 100644 F3:F303/MLX90640/usb_lib.h create mode 100644 F3:F303/MLX90640/version.inc diff --git a/F3:F303/MLX90640/Makefile b/F3:F303/MLX90640/Makefile new file mode 100644 index 0000000..75126ac --- /dev/null +++ b/F3:F303/MLX90640/Makefile @@ -0,0 +1,10 @@ +BINARY := mlx90640 +# MCU code +MCU := F303xb +# change this linking script depending on particular MCU model, +LDSCRIPT := stm32f303xB.ld +DEFINES := -DUSB1_16 +LDLIBS := -lm + +include ../makefile.f3 +include ../../makefile.stm32 diff --git a/F3:F303/MLX90640/hardware.c b/F3:F303/MLX90640/hardware.c new file mode 100644 index 0000000..7765ee6 --- /dev/null +++ b/F3:F303/MLX90640/hardware.c @@ -0,0 +1,34 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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" + +static inline void gpio_setup(){ + RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN; // for USART and LEDs + for(int i = 0; i < 10000; ++i) nop(); + // USB - alternate function 14 @ pins PA11/PA12; SWD - AF0 @PA13/14 + GPIOA->AFR[1] = AFRf(14, 11) | AFRf(14, 12); + GPIOA->MODER = MODER_AF(11) | MODER_AF(12) | MODER_AF(13) | MODER_AF(14) | MODER_O(15); + GPIOB->MODER = MODER_O(0) | MODER_O(1); + GPIOB->ODR = 1; +} + +void hw_setup(){ + gpio_setup(); +} + diff --git a/F3:F303/MLX90640/hardware.h b/F3:F303/MLX90640/hardware.h new file mode 100644 index 0000000..699d738 --- /dev/null +++ b/F3:F303/MLX90640/hardware.h @@ -0,0 +1,31 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 + +#include + +#define USBPU_port GPIOA +#define USBPU_pin (1<<15) +#define USBPU_ON() pin_clear(USBPU_port, USBPU_pin) +#define USBPU_OFF() pin_set(USBPU_port, USBPU_pin) + +extern volatile uint32_t Tms; + +void hw_setup(); + diff --git a/F3:F303/MLX90640/i2c.c b/F3:F303/MLX90640/i2c.c new file mode 100644 index 0000000..788a88b --- /dev/null +++ b/F3:F303/MLX90640/i2c.c @@ -0,0 +1,380 @@ +/* + * This file is part of the i2cscan project. + * Copyright 2023 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 +#include + +#include "i2c.h" +#include "strfunc.h" // hexdump +#include "usb_dev.h" + +i2c_speed_t i2c_curspeed = I2C_SPEED_AMOUNT; +extern volatile uint32_t Tms; +static uint32_t cntr; +volatile uint8_t i2c_scanmode = 0; // == 1 when I2C is in scan mode +static volatile uint8_t i2c_got_DMA = 0; // got DMA data +static uint8_t i2caddr = I2C_ADDREND; // current address in scan mode +static volatile int I2Cbusy = 0, goterr = 0; // busy==1 when DMA active, goterr==1 if 't was error @ last sent +static uint16_t I2Cbuf[I2C_BUFSIZE]; +static uint16_t i2cbuflen = 0; // buffer for DMA rx and its len +static volatile uint16_t dma_remain = 0; // remain bytes of DMA read/write + +// macros for I2C rx/tx +#define DMARXCCR (DMA_CCR_MINC | DMA_CCR_TCIE | DMA_CCR_TEIE) +#define DMATXCCR (DMA_CCR_MINC | DMA_CCR_DIR | DMA_CCR_TCIE | DMA_CCR_TEIE) +// macro for I2CCR1 +#define I2CCR1 (I2C_CR1_PE | I2C_CR1_RXDMAEN | I2C_CR1_TXDMAEN) + +// return 1 if I2Cbusy is set & timeout reached +static inline int isI2Cbusy(){ + cntr = Tms; + do{ + if(Tms - cntr > I2C_TIMEOUT){ U("Timeout, DMA transfer in progress?"); return 1;} + }while(I2Cbusy); + return 0; +} + +static void swapbytes(uint16_t *data, uint16_t datalen){ + if(!datalen) return; + for(int i = 0; i < datalen; ++i) + data[i] = __REV16(data[i]); +} + +// GPIO Resources: I2C1_SCL - PB6 (AF4), I2C1_SDA - PB7 (AF4) +void i2c_setup(i2c_speed_t speed){ + uint8_t PRESC, SCLDEL = 0x04, SDADEL = 0x03, SCLH, SCLL; // I2C1->TIMINGR fields + switch(speed){ + case I2C_SPEED_10K: + PRESC = 0x0F; + SCLH = 0xDA; + SCLL = 0xE0; + break; + case I2C_SPEED_100K: + PRESC = 0x0F; + SCLH = 0x13; + SCLL = 0x16; + break; + case I2C_SPEED_400K: + PRESC = 0x07; + SCLH = 0x08; + SCLL = 0x09; + break; + case I2C_SPEED_1M: + SDADEL = 1; + SCLDEL = 2; + PRESC = 0x3; + SCLH = 0x4; + SCLL = 0x6; + break; + case I2C_SPEED_2M: + SDADEL = 0; + SCLDEL = 1; + PRESC = 0x0; + SCLH = 0x1; + SCLL = 0x2; + break; + default: + USND("Wrong I2C speed!"); + return; // wrong speed + } + RCC->AHBENR |= RCC_AHBENR_GPIOBEN; + I2C1->CR1 = 0; // disable I2C for setup + I2C1->ICR = 0x3f38; // clear all errors + GPIOB->AFR[0] = (GPIOB->AFR[0] & ~(GPIO_AFRL_AFRL6 | GPIO_AFRL_AFRL7)) | + AFRf(4, 6) | AFRf(4, 7); + GPIOB->MODER = (GPIOB->MODER & ~(GPIO_MODER_MODER6 | GPIO_MODER_MODER7)) | + GPIO_MODER_MODER6_AF | GPIO_MODER_MODER7_AF; + GPIOB->PUPDR = (GPIOB->PUPDR & !(GPIO_PUPDR_PUPDR6 | GPIO_PUPDR_PUPDR7)) | + GPIO_PUPDR6_PU | GPIO_PUPDR7_PU; // pullup (what if there's no external pullup?) + GPIOB->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7; // both open-drain outputs + // I2C (default timing from sys clock - 72MHz) + RCC->APB1ENR |= RCC_APB1ENR_I2C1EN; // clocking + if(speed < I2C_SPEED_400K){ // slow cpeed - common mode + SYSCFG->CFGR1 &= ~(SYSCFG_CFGR1_I2C1_FMP | SYSCFG_CFGR1_I2C_PB6_FMP | SYSCFG_CFGR1_I2C_PB7_FMP); + }else{ // activate "fast mode plus" + SYSCFG->CFGR1 |= SYSCFG_CFGR1_I2C1_FMP | SYSCFG_CFGR1_I2C_PB6_FMP | SYSCFG_CFGR1_I2C_PB7_FMP; + } + I2C1->TIMINGR = (PRESC<CR1 = I2CCR1; + RCC->AHBENR |= RCC_AHBENR_DMA1EN; + NVIC_EnableIRQ(DMA1_Channel6_IRQn); + NVIC_EnableIRQ(DMA1_Channel7_IRQn); + I2Cbusy = 0; + i2c_curspeed = speed; +} + +// setup DMA for rx (tx==0) or tx (tx==1) +// DMA channels: 7 - I2C1_Rx, 6 - I2C1_Tx +static void i2cDMAsetup(int tx, uint16_t len){ + i2cbuflen = len; + if(len > 255) len = 255; + if(tx){ + DMA1_Channel6->CCR = DMATXCCR; + DMA1_Channel6->CPAR = (uint32_t) &I2C1->TXDR; + DMA1_Channel6->CMAR = (uint32_t) I2Cbuf; + DMA1_Channel6->CNDTR = len; + }else{ + DMA1_Channel7->CCR = DMARXCCR; + DMA1_Channel7->CPAR = (uint32_t) &I2C1->RXDR; + DMA1_Channel7->CMAR = (uint32_t) I2Cbuf; + DMA1_Channel7->CNDTR = len; + } +} + +// wait until bit set or clear; return 1 if OK, 0 in case of timeout +static uint8_t waitISRbit(uint32_t bit, uint8_t isset){ + uint32_t waitwhile = (isset) ? 0 : bit; // wait until != + cntr = Tms; + while((I2C1->ISR & bit) == waitwhile){ + IWDG->KR = IWDG_REFRESH; + if(I2C1->ISR & I2C_ISR_NACKF){ + goto goterr; + } + if(Tms - cntr > I2C_TIMEOUT){ + goto goterr; + } + } + return 1; +goterr: + I2C1->ICR = 0xff; + return 0; +} + +// start writing +static uint8_t i2c_startw(uint8_t addr, uint8_t nbytes, uint8_t stop){ + if(!waitISRbit(I2C_ISR_BUSY, 0)) return 0; + uint32_t cr2 = nbytes << 16 | addr | I2C_CR2_START; + if(stop) cr2 |= I2C_CR2_AUTOEND; + // now start transfer + I2C1->CR2 = cr2; + return 1; +} + +/** + * write command byte to I2C + * @param addr - device address + * @param data - bytes to write + * @param nbytes - amount of bytes to write + * @param stop - to set STOP + * @return 0 if error + */ +static uint8_t i2c_writes(uint8_t addr, uint8_t *data, uint8_t nbytes, uint8_t stop){ + if(!i2c_startw(addr, nbytes, stop)) return 0; + for(int i = 0; i < nbytes; ++i){ + cntr = Tms; + while(!(I2C1->ISR & I2C_ISR_TXIS)){ // ready to transmit + IWDG->KR = IWDG_REFRESH; + if(I2C1->ISR & I2C_ISR_NACKF){ + I2C1->ICR |= I2C_ICR_NACKCF; + return 0; + } + if(Tms - cntr > I2C_TIMEOUT){ + return 0; + } + } + I2C1->TXDR = data[i]; // send data + } + cntr = Tms; + if(stop){ + if(!waitISRbit(I2C_ISR_BUSY, 0)) return 0; + }else{ // repeated start + if(!waitISRbit(I2C_ISR_TC, 1)) return 0; + } + return 1; +} + +uint8_t i2c_write(uint8_t addr, uint16_t *data, uint8_t nwords){ + if(nwords < 1 || nwords > 127) return 0; + if(isI2Cbusy()) return 0; + uint16_t nbytes = nwords << 1; + swapbytes(data, nwords); + return i2c_writes(addr, (uint8_t*)data, nbytes, 1); +} + +uint8_t i2c_write_dma16(uint8_t addr, uint16_t *data, uint8_t nwords){ + if(!data || nwords < 1 || nwords > 127) return 0; + if(isI2Cbusy()) return 0; + uint16_t nbytes = nwords << 1; + swapbytes(data, nwords); + i2cDMAsetup(1, nbytes); + goterr = 0; + if(!i2c_startw(addr, nbytes, 1)) return 0; + I2Cbusy = 1; + DMA1_Channel6->CCR = DMATXCCR | DMA_CCR_EN; // start transfer + return 1; +} + +// start reading of `nbytes` from `addr`; if `start`==`, set START +static uint8_t i2c_startr(uint8_t addr, uint16_t nbytes, uint8_t start){ + uint32_t cr2 = addr | I2C_CR2_RD_WRN; + if(nbytes > 255) cr2 |= I2C_CR2_RELOAD | (0xff0000); + else cr2 |= I2C_CR2_AUTOEND | (nbytes << 16); + I2C1->CR2 = (start) ? cr2 | I2C_CR2_START : cr2; + return 1; +} + +/** + * read nbytes of data from I2C line + * all functions with `addr` should have addr = address << 1 + * `data` should be an array with at least `nbytes` length + * @return 1 if all OK, 0 if NACK or no device found + */ +static uint8_t *i2c_readb(uint8_t addr, uint16_t nbytes){ + uint8_t start = 1; + uint8_t *bptr = (uint8_t*)I2Cbuf; + while(nbytes){ + if(!i2c_startr(addr, nbytes, start)) return NULL; + if(nbytes < 256){ + for(int i = 0; i < nbytes; ++i){ + if(!waitISRbit(I2C_ISR_RXNE, 1)) return NULL; + *bptr++ = I2C1->RXDR; + } + break; + }else while(!(I2C1->ISR & I2C_ISR_TCR)){ // until first part read + if(!waitISRbit(I2C_ISR_RXNE, 1)) return NULL; + *bptr++ = I2C1->RXDR; + } + nbytes -= 255; + start = 0; + } + return (uint8_t*)I2Cbuf; +} + +uint8_t *i2c_read(uint8_t addr, uint16_t nbytes){ + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0)) return 0; + return i2c_readb(addr, nbytes); +} + +static uint8_t dmard(uint8_t addr, uint16_t nbytes){ + if(nbytes < 1 || nbytes > I2C_BUFSIZE) return 0; + i2cDMAsetup(0, nbytes); + goterr = 0; + i2c_got_DMA = 0; + (void) I2C1->RXDR; // avoid wrong first byte + DMA1_Channel7->CCR = DMARXCCR | DMA_CCR_EN; // init DMA before START sequence + if(!i2c_startr(addr, nbytes, 1)) return 0; + dma_remain = nbytes > 255 ? nbytes - 255 : 0; // remainder after first read finish + I2Cbusy = 1; + return 1; +} + +uint8_t i2c_read_dma16(uint8_t addr, uint16_t nwords){ + if(nwords > I2C_BUFSIZE/2) return 0; // what if `nwords` is very large? we should check it + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0)) return 0; + return dmard(addr, nwords<<1); +} + +// read 16bit register reg +uint16_t *i2c_read_reg16(uint8_t addr, uint16_t reg16, uint16_t nwords, uint8_t isdma){ + if(isI2Cbusy() || !waitISRbit(I2C_ISR_BUSY, 0) || nwords < 1 || nwords > I2C_BUFSIZE/2) return 0; + reg16 = __REV16(reg16); + if(!i2c_writes(addr, (uint8_t*)®16, 2, 0)) return NULL; + if(isdma){ + if(dmard(addr, nwords<<1)) return I2Cbuf; + return NULL; + } + if(!i2c_readb(addr, nwords<<1)) return NULL; + swapbytes((uint16_t*)I2Cbuf, nwords); + return (uint16_t*)I2Cbuf; +} + +void i2c_init_scan_mode(){ + i2caddr = 1; // start from 1 as 0 is a broadcast address + i2c_scanmode = 1; +} + +// return 1 if next addr is active & return in as `addr` +// if addresses are over, return 1 and set addr to I2C_NOADDR +// if scan mode inactive, return 0 and set addr to I2C_NOADDR +int i2c_scan_next_addr(uint8_t *addr){ + if(isI2Cbusy()) return 0; + *addr = i2caddr; + if(i2caddr == I2C_ADDREND){ + *addr = I2C_ADDREND; + i2c_scanmode = 0; + return 0; + } + if(!i2c_read((i2caddr++)<<1, 1)) return 0; + return 1; +} + +// dump I2Cbuf +void i2c_bufdudump(){ + if(goterr){ + USND("DMARDERR"); + goterr = 0; + } + if(i2cbuflen < 1) return; + USND("DMARD="); + hexdump16(USB_sendstr, (uint16_t*)I2Cbuf, i2cbuflen); +} + +// get DMA buffer with conversion to little-endian (if transfer was for 16-bit) +uint16_t *i2c_dma_getbuf(uint16_t *len){ + if(!i2c_got_DMA || i2cbuflen < 1) return NULL; + i2c_got_DMA = 0; + i2cbuflen >>= 1; // for hexdump16 - now buffer have uint16_t! + swapbytes((uint16_t*)I2Cbuf, i2cbuflen); + if(len) *len = i2cbuflen; + return I2Cbuf; +} + +int i2c_dma_haderr(){ + int r = goterr; + goterr = 0; + return r; +} + +int i2c_busy(){ return I2Cbusy;} + +// Rx (7) /Tx (6) interrupts +static void I2C_isr(int rx){ + uint32_t isr = DMA1->ISR; + DMA_Channel_TypeDef *ch = (rx) ? DMA1_Channel7 : DMA1_Channel6; + ch->CCR &= ~DMA_CCR_EN; // clear enable for further settings + if(isr & (DMA_ISR_TEIF6 | DMA_ISR_TEIF7)){ + goterr = 1; goto ret; + } + if(dma_remain){ // receive/send next portion + uint16_t len = (dma_remain > 255) ? 255 : dma_remain; + ch->CNDTR = len; + if(rx){ + if(!i2c_startr(0, dma_remain, 0)){ + goterr = 1; goto ret; + } + ch->CMAR += 255; + } + dma_remain -= len; + ch->CCR |= DMA_CCR_EN; + DMA1->IFCR = DMA_IFCR_CTCIF6 | DMA_IFCR_CTCIF7; + return; + }else if(rx) i2c_got_DMA = 1; // last transfer was Rx and all data read +ret: + ch->CCR = 0; + I2Cbusy = 0; + DMA1->IFCR = 0x0ff00000; // clear all flags for channel6/7 +} + +void dma1_channel6_isr(){ + I2C_isr(0); +} + +void dma1_channel7_isr(){ + I2C_isr(1); +} diff --git a/F3:F303/MLX90640/i2c.h b/F3:F303/MLX90640/i2c.h new file mode 100644 index 0000000..8692481 --- /dev/null +++ b/F3:F303/MLX90640/i2c.h @@ -0,0 +1,57 @@ +/* + * This file is part of the i2cscan project. + * Copyright 2023 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 +#include + +#define I2C_ADDREND (0x80) +// size in words +#define I2C_BUFSIZE (1024) + +typedef enum{ + I2C_SPEED_10K, + I2C_SPEED_100K, + I2C_SPEED_400K, + I2C_SPEED_1M, + I2C_SPEED_2M, // EXPERIMENTAL! Could be unstable!!! (speed near 1.9Mbaud) + I2C_SPEED_AMOUNT +} i2c_speed_t; + +extern i2c_speed_t i2c_curspeed; +extern volatile uint8_t i2c_scanmode; + +// timeout of I2C bus in ms +#define I2C_TIMEOUT (5) + +void i2c_setup(i2c_speed_t speed); +int i2c_busy(); + +uint8_t *i2c_read(uint8_t addr, uint16_t nbytes); +uint8_t i2c_read_dma16(uint8_t addr, uint16_t nwords); +uint16_t *i2c_read_reg16(uint8_t addr, uint16_t reg16, uint16_t nbytes, uint8_t isdma); + +uint8_t i2c_write(uint8_t addr, uint16_t *data, uint8_t nwords); +uint8_t i2c_write_dma16(uint8_t addr, uint16_t *data, uint8_t nwords); + +void i2c_bufdudump(); +int i2c_dma_haderr(); +uint16_t *i2c_dma_getbuf(uint16_t *len); +int i2c_getwords(uint16_t *buf, int bufsz); + +void i2c_init_scan_mode(); +int i2c_scan_next_addr(uint8_t *addr); diff --git a/F3:F303/MLX90640/main.c b/F3:F303/MLX90640/main.c new file mode 100644 index 0000000..5d4188b --- /dev/null +++ b/F3:F303/MLX90640/main.c @@ -0,0 +1,68 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 "i2c.h" +#include "proto.h" +#include "strfunc.h" +#include "usb_dev.h" + +#define MAXSTRLEN RBINSZ + +volatile uint32_t Tms = 0; + +void sys_tick_handler(void){ + ++Tms; +} + +int main(void){ + char inbuff[MAXSTRLEN+1]; + if(StartHSE()){ + SysTick_Config((uint32_t)72000); // 1ms + }else{ + StartHSI(); + SysTick_Config((uint32_t)48000); // 1ms + } + USBPU_OFF(); + hw_setup(); + i2c_setup(I2C_SPEED_100K); + USB_setup(); + USBPU_ON(); + uint32_t ctr = Tms; + while(1){ + if(Tms - ctr > 499){ + ctr = Tms; + pin_toggle(GPIOB, 1 << 1 | 1 << 0); // toggle LED @ PB0 + } + int l = USB_receivestr(inbuff, MAXSTRLEN); + if(l < 0) USB_sendstr("USBOVERFLOW\n"); + else if(l){ + const char *ans = parse_cmd(inbuff); + if(ans) USB_sendstr(ans); + } + if(i2c_scanmode){ + uint8_t addr; + int ok = i2c_scan_next_addr(&addr); + if(addr == I2C_ADDREND) USND("SCANEND"); + else if(ok){ + U("FOUNDID="); + USND(uhex2str(addr)); + } + } + } +} diff --git a/F3:F303/MLX90640/mlx90640.bin b/F3:F303/MLX90640/mlx90640.bin new file mode 100755 index 0000000000000000000000000000000000000000..9149985a9237743bc2ee8b81a2b074271fc6401d GIT binary patch literal 7284 zcmds5dstN0wcq=kGfxH?L<|fV&j1==P)A5oj7d2R!*L$q7%}mc#~B`RMnMorOYiNy z4rm%Qrcp4BXp$ODTJ!J$>P<{ShpS0$ZfozIQE4HXGL6wOjA_q=QI3etU3-u=P5QAzrEI8Ywxx8puLElk${v3DWrV!I|+T~BQzwDZc>&i|9;MQG5%3ouR_Wh zkp2D5}Lk1C`?n;D}-%`1@@!^@@{euZ6O zTj_k*^|-{?E!T^h;-+IywkaIro|=$B9+&5A-ET7iEW;g)zr7G(=em{T%g^vKMy z_8W?@XOZk0_lV+J-=aFC3Zs?9QsxrXHEtyiZK59(fJSl*TUChl+{?{!M3~|h;T6w+ zNJ`HsNxA(USoenTnb3Ph+(fY?N8XrRAlvOA@-{3&-ooQKcEqOFrc|%0eMI)XbF=KL zYPHO?{?)ZvHd;)eQ!x&EifLGlf`qU273;Qs>Kb$jBPvSQhP&GrrNRt9Bk#|Oc$U>i zJWI1g4YR5`33{i>H2qVpSrJrihB3s}M^I&}v=^CU6mjcU>NHO@x*xLF=O%&t!M z3|1>_k5yxRbo9w6%KqB#h-l15gG3LLysu=ClI-hN(5{*<%H4EiJ6||BvwWtbl{fQT zUbdIZdcDWO>-p%YK6)U;P#rz5nR8FP7cNSm2IwPXC$e41cJ6x72F$gJMw}zquD>;# z3gCs`7bstCt)98jzpgfgxzbPO&;uy$malNHCF)VU_&pGV&7s zkP&f3rUYbaPg3KBGJ|el=n5?+Cn{1?>67gmi(0=wc(#WzBn+snC)p^9v-VHbbJ;F$KF!k{_{C7`Fchj+dm@ z*^?v1N!U>3qYO{@kl|t920O}FCFIKE`)jT5N|)K+)Y9TcdbdgDGl?DwW&XPQS!u6v zvuLE~fI*&f?kQU?R|2Odi4$y_B3wn1ka$Gwy@xs7KP0vM`-hTNAY7sBk9`*;rEyVu zC2++3lZEVnqUr)s-lb+UG+>GYkoPgaAK7(WhX%MSJX zRa3>8l+A~87y3}nFZ}z0iSqsDejcR6nKU*yhR;lAd}IfsqZzkf3ce)0VPqH526;>y zcO@u6l4qqWMl^SeKLn${Fiy1E{9gtOEDEqY#cecqW8|L?UkEC)L;iVo+=X_fwyHE; zjVlB)vi|UY2T@K@WX)lMEjMh8)EriXUq{hVD*9w>z1O;!^Ji#|4ecDIn$^tOU0rWDIH|%>o zY?RAF&(Ny5OsItZVxXp!8u*j{zPzi0_2R~oLOxCqKWtc3Y4{Ad!THGWpbs#z)B%*| z7&bFk&(Ajy9+Y0Qh?z9Pg$&Oj>uUtBkIY1wOVZYo|zf;a9=C?a_?SoQQb3R*3R-jP{4&hWfzzpKx+ z%k-XiM3>CJ<2|KIOKH6OtnO`70-t(5{zNt}_1>>LWXg0M@XqD)`4lq}-@bjwH0At7 zSEb8fz9?LDb(%uZ+O7LSAk=6U$ywKl)VI=*_~Py1pmcnC*-mM^FzLij=~f|qLh4N_ zhg$ZbwP$+J+)+QihqD1&#l)ye`F+AC!BYMs*l#1qerer~BX69*nM^njU958RTt;;C z&FG~n=I#Wa^1O?GttUFVX)NA;O5&`EM~uB{@aIs^A$3K0zr-!z4B#0vKZY9VBk|F? zvFkwuaufN9+(Xhb&*me$z(1-pXfcYOmlD`L-3eu|x8XebrFA$XKAeD!JeniiIgXNjd*@E4jTvj4t+7a>GZ+A|Iwa|Q&xmn^+qVTaK z8Kqt|U}eOx27+;x=;))--YXLK1&M#PaEFomfI*gzAYMfXbLfvKFe&em6r@bBJ0cC# zDXE=BUhjzG1@B1x3vqUh^*NLLOq>lOk~kVt7re_1J(h#tRF}VHM|# zBWB4+w0DX*X18KX2sJoyupKcm&>w%?)L zPBZ8JO8K01i(Tz{&XOVCMVZ8Dlv-FJkX0apzO%|stUlLzXKn}n_S}E6Pw;LMDd1nX zj?B_hx+)?GC2A;TuEy7BmBnP4X#Z83$k}>(y`{mrQV_Eo;`U;Xl*v71L$efFphfN# zAzf&9R=7(deVG+b(>K~C*O%;YVz5OPW5Mq(jv(xJ=2cIiFR?8yf4D5En(zZQ zwFa=Bf?JqNsySO-QGHkSA>fG#kJH2tTgrzdWvj(OcgBZT#}N;|C++C9v5lIuWOP89 zX4Em~OQ;B!Pm462RPw2mVuW|tIPnL_z==COZ>~i-oH*pcnQvESuU)rxmkEjXHg4pA zXV~3l;zr)^^h+rL6pk*K5~4IVUK5T!*l7N*Fji54ou%MmJSy#52Qk*PGGNE8{40m+9?FZsavjoP><8 zNRP27cZKg2>2V{8x3gNUxkxN%b?0*8l-Bt0LBZI*=iGF<$-c+EuuiS(`PHb)^dR(_Q zcWtNX1Md}gn{JP8PYMFAyG(ESzbnJ5!8Qw6SS8U1JIfHf59&&Xlr@*7vW+e*`XpGzs*RpDsCQLvL~XYlS9 zrQ*H7kM^LH&N6o-M`Cs>k$pArb8T=bta@a88RQ4Y7Y3KaqOu=fe%P`ZG!h-%4_wS2 zZ@#=G-ebos z_rrRyf#*H1Na@BLiCQS<6>l10kB><+$F_#mFb8E1`|+#fRsG|b^-WsHKYhzWlpy1b z@i@!6AkK`p5IZ9KQymO|Zy`R%2c-mwoDbrQc0%7r(B2Crn~Ib>AqF@NZK6#JZ~(fM zP`26ET59qs_G~m1UH|^!)#w?i*!TvlbmgXE7sNL=qN5jYMn^Z?tiQb1f3GH1F9!(K6bgwG9)#Bap)$Ml*oUkq(W^P95&MXLuIro2TAy$ z@W7Sen|JKID6;L4LyQ*eyo_IE^=)uoijAw4iq2mJZ3yb2 z!42`A=Q(#$1eN^RKXYtL_{24|5$`Cve(F%iSjV{H0OqNTC7?I3)4u5F%Q58Rhj5n_ zZRuG8+RBXeymN@&GbHVL?YELHGr{6*qJ2 zCpR=<8Y3}*Qsq)%tm42n34<3I8Ed^nbvi^f4Ks~k zy->=GiUV-|#|Y*UMD6kPbo9iBXA*uiABI&48sKQBmvEyAFW1C})2FnPR^Lx2d3#Xi zbMnDpO6)8rUM8}=8$2eBYXODElTx9b-js&r`HBPWUR}d36YNwX9g{1GzY6!^^AZdE z?T4Fa{@A=rSHX7E!Hzq?jyoYu+B32B7Wl95x2I2^=HUIQHt#TjpDRHAhDF-r@H(_Z z6y?|fah0~0GShsgiHwlQ>PB#RWBvLytTn%cZEC2gsn#R-N|!EGlhR>Vqem)Cjz>$E z@@0gF{y*A?V9IqMo~YntXYH`dh5 zt8+Fr*EG)K3m)L%Uz2%twJi&b_up$|E1!1PR%gyHU^zD5n1A0KYVGH zYijCh);F{DPq9tz^=oQtSnE=@vc9&yt_Hx^^0E~%5VhMlcWwbYhi$59e!3yH0Y3zm zCU$|HZ(NrL8OiUB<@3wg`bIXtoXxIjX{c!gVb?c1YxOy5tCMwBS2s=ov21nC58PEX zu`ZBVZEa*5Yu2#qVYsozSq+sR);Cr+u}?MD*Rk{OpHt~>CIoIGr`r0%JEMfD_0@M8 z*{+(FdDS&l^XjXb=T*Ab=djK7gk80@i7Z;>Tpwe;^64h^GU#Y%bc66!Pd7G_O;}df z1X|-Ak1RG@V=6-xCB=(N7n{+1qa6}t@PUZ2a(+2N`H-e!E&elwxlIhW98s8pQ;1Yl z2~?3MU&TOJLQ$BJWg#t=WnqMwEQ*q4$q`n_w$f^(RWTS*Y6>BRtd-(Ofk}b#AZDKi z=>piQ57K*()N(BlV=!BU5)u?N6o_FMSPCJUCMzTKS&|YE5eiIK$pIA|fcyL%NXH<3 z2&oT}5k8iM!T;qqovuT*s0BTVnotc|4|zRm1S|~=e-uN|H5Z;j_tWL546Q&5pnX5O z7jpJ*s998poG{mk)&NFb45t||Qs^2~4Zj~D4(OeLJQuK^MhvZDIKuBqBApT$TS#N# V8-Vi-ap2#41Mz>~Cwt0+{s$?0$V31D literal 0 HcmV?d00001 diff --git a/F3:F303/MLX90640/mlx90640.cflags b/F3:F303/MLX90640/mlx90640.cflags new file mode 100644 index 0000000..68d5165 --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.cflags @@ -0,0 +1 @@ +-std=c17 \ No newline at end of file diff --git a/F3:F303/MLX90640/mlx90640.config b/F3:F303/MLX90640/mlx90640.config new file mode 100644 index 0000000..1cf1964 --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.config @@ -0,0 +1,7 @@ +// Add predefined macros for your project here. For example: +// #define THE_ANSWER 42 +#define EBUG +#define STM32F3 +#define STM32F303xb +#define __thumb2__ 1 +#define __ARM_ARCH_7M__ diff --git a/F3:F303/MLX90640/mlx90640.creator b/F3:F303/MLX90640/mlx90640.creator new file mode 100644 index 0000000..e94cbbd --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.creator @@ -0,0 +1 @@ +[General] diff --git a/F3:F303/MLX90640/mlx90640.creator.user b/F3:F303/MLX90640/mlx90640.creator.user new file mode 100644 index 0000000..19c5b4c --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.creator.user @@ -0,0 +1,215 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + true + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 0 + 80 + true + true + 1 + 0 + false + true + false + 0 + true + true + 0 + 8 + true + false + 1 + true + false + true + *.md, *.MD, Makefile + false + true + true + + + + ProjectExplorer.Project.PluginSettings + + + true + false + true + true + true + true + + false + + + 0 + true + + true + true + Builtin.DefaultTidyAndClazy + 2 + false + + + + true + + + + + ProjectExplorer.Project.Target.0 + + Desktop + true + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Electronics/STM32/F303-nolib/blink + + + + all + + true + GenericProjectManager.GenericMakeStep + + 1 + Build + Build + ProjectExplorer.BuildSteps.Build + + + + + clean + + true + GenericProjectManager.GenericMakeStep + + 1 + Clean + Clean + ProjectExplorer.BuildSteps.Clean + + 2 + false + + false + + Default + GenericProjectManager.GenericBuildConfiguration + 0 + 0 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + 1 + + + 0 + Deploy + Deploy + ProjectExplorer.BuildSteps.Deploy + + 1 + + false + ProjectExplorer.DefaultDeployConfiguration + + 1 + + true + true + true + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + + ProjectExplorer.CustomExecutableRunConfiguration + + false + true + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 22 + + + Version + 22 + + diff --git a/F3:F303/MLX90640/mlx90640.cxxflags b/F3:F303/MLX90640/mlx90640.cxxflags new file mode 100644 index 0000000..6435dfc --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.cxxflags @@ -0,0 +1 @@ +-std=c++17 \ No newline at end of file diff --git a/F3:F303/MLX90640/mlx90640.files b/F3:F303/MLX90640/mlx90640.files new file mode 100644 index 0000000..9c687cb --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.files @@ -0,0 +1,20 @@ +hardware.c +hardware.h +i2c.c +i2c.h +main.c +proto.c +proto.h +ringbuffer.c +ringbuffer.h +strfunc.c +strfunc.h +usb.c +usb.h +usb_descr.c +usb_descr.h +usb_dev.c +usb_dev.h +usb_lib.c +usb_lib.h +usbhw.h diff --git a/F3:F303/MLX90640/mlx90640.includes b/F3:F303/MLX90640/mlx90640.includes new file mode 100644 index 0000000..06d1130 --- /dev/null +++ b/F3:F303/MLX90640/mlx90640.includes @@ -0,0 +1,6 @@ +. +../inc +../inc/Fx +../inc/cm +../inc/ld +../inc/startup diff --git a/F3:F303/MLX90640/openocd.cfg b/F3:F303/MLX90640/openocd.cfg new file mode 100644 index 0000000..56ccc2e --- /dev/null +++ b/F3:F303/MLX90640/openocd.cfg @@ -0,0 +1,119 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +# script for stm32f3x family + +# +# stm32 devices support both JTAG and SWD transports. +# +source [find interface/stlink-v2-1.cfg] +source [find target/swj-dp.tcl] +source [find mem_helper.tcl] + +if { [info exists CHIPNAME] } { + set _CHIPNAME $CHIPNAME +} else { + set _CHIPNAME stm32f3x +} + +set _ENDIAN little + +# Work-area is a space in RAM used for flash programming +# By default use 16kB +if { [info exists WORKAREASIZE] } { + set _WORKAREASIZE $WORKAREASIZE +} else { + set _WORKAREASIZE 0x4000 +} + +# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz +# +# Since we may be running of an RC oscilator, we crank down the speed a +# bit more to be on the safe side. Perhaps superstition, but if are +# running off a crystal, we can run closer to the limit. Note +# that there can be a pretty wide band where things are more or less stable. +adapter speed 1000 + +adapter srst delay 100 +if {[using_jtag]} { + jtag_ntrst_delay 100 +} + +#jtag scan chain +if { [info exists CPUTAPID] } { + set _CPUTAPID $CPUTAPID +} else { + if { [using_jtag] } { + # See STM Document RM0316 + # Section 29.6.3 - corresponds to Cortex-M4 r0p1 + set _CPUTAPID 0x4ba00477 + } { + set _CPUTAPID 0x2ba01477 + } +} + +swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID +dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu + +if {[using_jtag]} { + jtag newtap $_CHIPNAME bs -irlen 5 +} + +set _TARGETNAME $_CHIPNAME.cpu +target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap + +$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +set _FLASHNAME $_CHIPNAME.flash +flash bank $_FLASHNAME stm32f1x 0 0 0 0 $_TARGETNAME + +reset_config srst_nogate + +if {![using_hla]} { + # if srst is not fitted use SYSRESETREQ to + # perform a soft reset + cortex_m reset_config sysresetreq +} + +proc stm32f3x_default_reset_start {} { + # Reset clock is HSI (8 MHz) + adapter speed 1000 +} + +proc stm32f3x_default_examine_end {} { + # Enable debug during low power modes (uses more power) + mmw 0xe0042004 0x00000007 0 ;# DBGMCU_CR |= DBG_STANDBY | DBG_STOP | DBG_SLEEP + + # Stop watchdog counters during halt + mmw 0xe0042008 0x00001800 0 ;# DBGMCU_APB1_FZ |= DBG_IWDG_STOP | DBG_WWDG_STOP +} + +proc stm32f3x_default_reset_init {} { + # Configure PLL to boost clock to HSI x 8 (64 MHz) + mww 0x40021004 0x00380400 ;# RCC_CFGR = PLLMUL[3:1] | PPRE1[2] + mmw 0x40021000 0x01000000 0 ;# RCC_CR |= PLLON + mww 0x40022000 0x00000012 ;# FLASH_ACR = PRFTBE | LATENCY[1] + sleep 10 ;# Wait for PLL to lock + mmw 0x40021004 0x00000002 0 ;# RCC_CFGR |= SW[1] + + # Boost JTAG frequency + adapter speed 8000 +} + +# Default hooks +$_TARGETNAME configure -event examine-end { stm32f3x_default_examine_end } +$_TARGETNAME configure -event reset-start { stm32f3x_default_reset_start } +$_TARGETNAME configure -event reset-init { stm32f3x_default_reset_init } + +tpiu create $_CHIPNAME.tpiu -dap $_CHIPNAME.dap -ap-num 0 -baseaddr 0xE0040000 + +lappend _telnet_autocomplete_skip _proc_pre_enable_$_CHIPNAME.tpiu +proc _proc_pre_enable_$_CHIPNAME.tpiu {_targetname} { + targets $_targetname + + # Set TRACE_IOEN; TRACE_MODE is set to async; when using sync + # change this value accordingly to configure trace pins + # assignment + mmw 0xe0042004 0x00000020 0 +} + +$_CHIPNAME.tpiu configure -event pre-enable "_proc_pre_enable_$_CHIPNAME.tpiu $_TARGETNAME" diff --git a/F3:F303/MLX90640/proto.c b/F3:F303/MLX90640/proto.c new file mode 100644 index 0000000..869969c --- /dev/null +++ b/F3:F303/MLX90640/proto.c @@ -0,0 +1,156 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 +#include + +#include "i2c.h" +#include "strfunc.h" +#include "usb_dev.h" +#include "version.inc" + +#define LOCBUFFSZ (32) +// local buffer for I2C data to send +static uint16_t locBuffer[LOCBUFFSZ]; +static uint8_t I2Caddress = 0x33 << 1; +extern volatile uint32_t Tms; + +static const char *OK = "OK\n", *ERR = "ERR\n"; +const char *helpstring = + "https://github.com/eddyem/stm32samples/tree/master/F3:F303/mlx90640 build#" BUILD_NUMBER " @ " BUILD_DATE "\n" + " management of single IR bolometer MLX90640\n" + "i0..3 - setup I2C with speed 10k, 100k, 400k, 1M or 2M (experimental!)\n" + "Ia addr - set device address\n" + "Ir reg n - read n words from 16-bit register\n" + "Iw words - send words (hex/dec/oct/bin) to I2C\n" + "Is - scan I2C bus\n" + "T - print current Tms\n" +; + +TRUE_INLINE const char *setupI2C(char *buf){ + static const char *speeds[I2C_SPEED_AMOUNT] = { + [I2C_SPEED_10K] = "10K", + [I2C_SPEED_100K] = "100K", + [I2C_SPEED_400K] = "400K", + [I2C_SPEED_1M] = "1M", + [I2C_SPEED_2M] = "2M" + }; + if(buf && *buf){ + buf = omit_spaces(buf); + int speed = *buf - '0'; + if(speed < 0 || speed >= I2C_SPEED_AMOUNT){ + return ERR; + } + i2c_setup((i2c_speed_t)speed); + } + U("I2CSPEED="); USND(speeds[i2c_curspeed]); + return NULL; +} + +TRUE_INLINE const char *chaddr(const char *buf){ + uint32_t addr; + const char *nxt = getnum(buf, &addr); + if(nxt && nxt != buf){ + if(addr > 0x7f) return ERR; + I2Caddress = (uint8_t) addr << 1; + }else addr = I2Caddress >> 1; + U("I2CADDR="); USND(uhex2str(addr)); + return NULL; +} + +// read I2C register[s] - only blocking read! (DMA allowable just for config/image reading in main process) +static const char *rdI2C(const char *buf){ + uint32_t N = 0; + const char *nxt = getnum(buf, &N); + if(!nxt || buf == nxt || N > 0xffff) return ERR; + buf = nxt; + uint16_t reg = N, *b16 = NULL; + nxt = getnum(buf, &N); + if(!nxt || buf == nxt || N == 0 || N > I2C_BUFSIZE) return ERR; + if(!(b16 = i2c_read_reg16(I2Caddress, reg, N, 0))) return ERR; + if(N == 1){ + char b[5]; + u16s(*b16, b); + b[4] = 0; + USND(b); + }else hexdump16(USB_sendstr, b16, N); + return NULL; +} + +// read N numbers from buf, @return 0 if wrong or none +TRUE_INLINE uint16_t readNnumbers(const char *buf){ + uint32_t D; + const char *nxt; + uint16_t N = 0; + while((nxt = getnum(buf, &D)) && nxt != buf && N < LOCBUFFSZ){ + buf = nxt; + locBuffer[N++] = (uint8_t) D&0xff; + } + return N; +} + +static const char *wrI2C(const char *buf){ + uint16_t N = readNnumbers(buf); + if(N == 0) return ERR; + if(!i2c_write(I2Caddress, locBuffer, N)) return ERR; + return OK; +} + +const char *parse_cmd(char *buf){ + if(!buf || !*buf) return NULL; + if(buf[1]){ + switch(*buf){ // "long" commands + case 'i': + return setupI2C(buf + 1); + case 'I': + buf = omit_spaces(buf + 1); + switch(*buf){ + case 'a': + return chaddr(buf + 1); + case 'r': + return rdI2C(buf + 1); + case 'w': + return wrI2C(buf + 1); + case 's': + i2c_init_scan_mode(); + return OK; + default: + return ERR; + } + break; + default: + return ERR; + } + } + switch(*buf){ // "short" (one letter) commands + case 'i': return setupI2C(NULL); // current settings + case 'T': + U("T="); + USND(u2str(Tms)); + break; + case '?': // help + case 'h': + case 'H': + U(helpstring); + break; + default: + return ERR; + break; + } + return NULL; +} diff --git a/F3:F303/MLX90640/proto.h b/F3:F303/MLX90640/proto.h new file mode 100644 index 0000000..72c0e7d --- /dev/null +++ b/F3:F303/MLX90640/proto.h @@ -0,0 +1,22 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 + +char *parse_cmd(char *buf); + diff --git a/F3:F303/MLX90640/ringbuffer.c b/F3:F303/MLX90640/ringbuffer.c new file mode 100644 index 0000000..41e13ac --- /dev/null +++ b/F3:F303/MLX90640/ringbuffer.c @@ -0,0 +1,163 @@ +/* + * Copyright 2023 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 + +#include "ringbuffer.h" + +static int datalen(ringbuffer *b){ + if(b->tail >= b->head) return (b->tail - b->head); + else return (b->length - b->head + b->tail); +} + +// stored data length +int RB_datalen(ringbuffer *b){ + if(b->busy) return -1; + b->busy = 1; + int l = datalen(b); + b->busy = 0; + return l; +} + +static int hasbyte(ringbuffer *b, uint8_t byte){ + if(b->head == b->tail) return -1; // no data in buffer + int startidx = b->head; + if(b->head > b->tail){ // + for(int found = b->head; found < b->length; ++found) + if(b->data[found] == byte) return found; + startidx = 0; + } + for(int found = startidx; found < b->tail; ++found) + if(b->data[found] == byte) return found; + return -1; +} + +/** + * @brief RB_hasbyte - check if buffer has given byte stored + * @param b - buffer + * @param byte - byte to find + * @return index if found, -1 if none or busy + */ +int RB_hasbyte(ringbuffer *b, uint8_t byte){ + if(b->busy) return -1; + b->busy = 1; + int ret = hasbyte(b, byte); + b->busy = 0; + return ret; +} + +// increment head or tail +TRUE_INLINE void incr(ringbuffer *b, volatile int *what, int n){ + *what += n; + if(*what >= b->length) *what -= b->length; +} + +static int read(ringbuffer *b, uint8_t *s, int len){ + int l = datalen(b); + if(!l) return 0; + if(l > len) l = len; + int _1st = b->length - b->head; + if(_1st > l) _1st = l; + if(_1st > len) _1st = len; + memcpy(s, b->data + b->head, _1st); + if(_1st < len && l > _1st){ + memcpy(s+_1st, b->data, l - _1st); + incr(b, &b->head, l); + return l; + } + incr(b, &b->head, _1st); + return _1st; +} + +/** + * @brief RB_read - read data from ringbuffer + * @param b - buffer + * @param s - array to write data + * @param len - max len of `s` + * @return bytes read or -1 if busy + */ +int RB_read(ringbuffer *b, uint8_t *s, int len){ + if(b->busy) return -1; + b->busy = 1; + int r = read(b, s, len); + b->busy = 0; + return r; +} + +static int readto(ringbuffer *b, uint8_t byte, uint8_t *s, int len){ + int idx = hasbyte(b, byte); + if(idx < 0) return 0; + int partlen = idx + 1 - b->head; + // now calculate length of new data portion + if(idx < b->head) partlen += b->length; + if(partlen > len) return -read(b, s, len); + return read(b, s, partlen); +} + +/** + * @brief RB_readto fill array `s` with data until byte `byte` (with it) + * @param b - ringbuffer + * @param byte - check byte + * @param s - buffer to write data + * @param len - length of `s` + * @return amount of bytes written (negative, if lenbusy) return -1; + b->busy = 1; + int n = readto(b, byte, s, len); + b->busy = 0; + return n; +} + +static int write(ringbuffer *b, const uint8_t *str, int l){ + int r = b->length - 1 - datalen(b); // rest length + if(l > r || !l) return 0; + int _1st = b->length - b->tail; + if(_1st > l) _1st = l; + memcpy(b->data + b->tail, str, _1st); + if(_1st < l){ // add another piece from start + memcpy(b->data, str+_1st, l-_1st); + } + incr(b, &b->tail, l); + return l; +} + +/** + * @brief RB_write - write some data to ringbuffer + * @param b - buffer + * @param str - data + * @param l - length + * @return amount of bytes written or -1 if busy + */ +int RB_write(ringbuffer *b, const uint8_t *str, int l){ + if(b->busy) return -1; + b->busy = 1; + int w = write(b, str, l); + b->busy = 0; + return w; +} + +// just delete all information in buffer `b` +int RB_clearbuf(ringbuffer *b){ + if(b->busy) return -1; + b->busy = 1; + b->head = 0; + b->tail = 0; + b->busy = 0; + return 1; +} diff --git a/F3:F303/MLX90640/ringbuffer.h b/F3:F303/MLX90640/ringbuffer.h new file mode 100644 index 0000000..ed2cf95 --- /dev/null +++ b/F3:F303/MLX90640/ringbuffer.h @@ -0,0 +1,41 @@ +/* + * Copyright 2023 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 + +#if defined STM32F0 +#include +#elif defined STM32F1 +#include +#elif defined STM32F3 +#include +#endif + +typedef struct{ + uint8_t *data; // data buffer + const int length; // its length + int head; // head index + int tail; // tail index + volatile int busy; // == TRUE if buffer is busy now +} ringbuffer; + +int RB_read(ringbuffer *b, uint8_t *s, int len); +int RB_readto(ringbuffer *b, uint8_t byte, uint8_t *s, int len); +int RB_hasbyte(ringbuffer *b, uint8_t byte); +int RB_write(ringbuffer *b, const uint8_t *str, int l); +int RB_datalen(ringbuffer *b); +int RB_clearbuf(ringbuffer *b); diff --git a/F3:F303/MLX90640/strfunc.c b/F3:F303/MLX90640/strfunc.c new file mode 100644 index 0000000..e8bbc6a --- /dev/null +++ b/F3:F303/MLX90640/strfunc.c @@ -0,0 +1,306 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 "strfunc.h" + +// hex line number for hexdumps +void u16s(uint16_t n, char *buf){ + for(int j = 3; j > -1; --j){ + register uint8_t q = n & 0xf; + n >>= 4; + if(q < 10) buf[j] = q + '0'; + else buf[j] = q - 10 + 'a'; + } +} + +/** + * @brief hexdump - dump hex array by 16 bytes in string + * @param sendfun - function to send data + * @param arr - array to dump + * @param len - length of `arr` + */ +void hexdump(int (*sendfun)(const char *s), uint8_t *arr, uint16_t len){ + char buf[64] = "0000 ", *bptr = &buf[6]; + for(uint16_t l = 0; l < len; ++l, ++arr){ + for(int16_t j = 1; j > -1; --j){ + register uint8_t half = (*arr >> (4*j)) & 0x0f; + if(half < 10) *bptr++ = half + '0'; + else *bptr++ = half - 10 + 'a'; + } + if((l & 0xf) == 0xf){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + u16s(l + 1, buf); + bptr = &buf[6]; + }else *bptr++ = ' '; + } + if(bptr != &buf[6]){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + } +} + +// dump uint16_t by 8 values in string +void hexdump16(int (*sendfun)(const char *s), uint16_t *arr, uint16_t len){ + char buf[64] = "0000 ", *bptr = &buf[6]; + for(uint16_t l = 0; l < len; ++l, ++arr){ + //uint16_t val = *arr; + u16s(*arr, bptr); + /*for(int16_t j = 3; j > -1; --j){ + register uint8_t q = val & 0xf; + val >>= 4; + if(q < 10) bptr[j] = q + '0'; + else bptr[j] = q - 10 + 'a'; + }*/ + bptr += 4; + if((l & 7) == 7){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + u16s((l + 1)*2, buf); // number of byte, not word! + bptr = &buf[6]; + }else *bptr++ = ' '; + } + if(bptr != &buf[6]){ + *bptr++ = '\n'; + *bptr = 0; + sendfun(buf); + } +} + +/** + * @brief _2str - convert value into string buffer + * @param val - |value| + * @param minus - ==0 if value > 0 + * @return buffer with number + */ +static const char *_2str(uint32_t val, uint8_t minus){ + static char strbuf[12]; + char *bufptr = &strbuf[11]; + *bufptr = 0; + if(!val){ + *(--bufptr) = '0'; + }else{ + while(val){ + uint32_t x = val / 10; + *(--bufptr) = (val - 10*x) + '0'; + val = x; + //*(--bufptr) = val % 10 + '0'; + //val /= 10; + } + } + if(minus) *(--bufptr) = '-'; + return bufptr; +} + +// return string with number `val` +const char *u2str(uint32_t val){ + return _2str(val, 0); +} +const char *i2str(int32_t i){ + uint8_t minus = 0; + uint32_t val; + if(i < 0){ + minus = 1; + val = -i; + }else val = i; + return _2str(val, minus); +} + +/** + * @brief uhex2str - print 32bit unsigned int as hex + * @param val - value + * @return string with number + */ +const char *uhex2str(uint32_t val){ + static char buf[12] = "0x"; + int npos = 2; + uint8_t *ptr = (uint8_t*)&val + 3; + int8_t i, j, z=1; + for(i = 0; i < 4; ++i, --ptr){ + if(*ptr == 0){ // omit leading zeros + if(i == 3) z = 0; + if(z) continue; + } + else z = 0; + for(j = 1; j > -1; --j){ + uint8_t half = (*ptr >> (4*j)) & 0x0f; + if(half < 10) buf[npos++] = half + '0'; + else buf[npos++] = half - 10 + 'a'; + } + } + buf[npos] = 0; + return buf; +} + +/** + * @brief omit_spaces - eliminate leading spaces and other trash in string + * @param buf - string + * @return - pointer to first character in `buf` > ' ' + */ +char *omit_spaces(const char *buf){ + while(*buf){ + if(*buf > ' ') break; + ++buf; + } + return (char*)buf; +} + +/** + * @brief getdec - read decimal number & return pointer to next non-number symbol + * @param buf - string + * @param N - number read + * @return Next non-number symbol. In case of overflow return `buf` and N==0xffffffff + */ +static const char *getdec(const char *buf, uint32_t *N){ + char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '9'){ + break; + } + if(num > 429496729 || (num == 429496729 && c > '5')){ // overflow + *N = 0xffffff; + return start; + } + num *= 10; + num += c - '0'; + ++buf; + } + *N = num; + return buf; +} +// read hexadecimal number (without 0x prefix!) +static const char *gethex(const char *buf, uint32_t *N){ + const char *start = buf; + 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){ + if(num & 0xf0000000){ // overflow + *N = 0xffffff; + return start; + } + num <<= 4; + num += c - M; + }else{ + break; + } + ++buf; + } + *N = num; + return buf; +} +// read octal number (without 0 prefix!) +static const char *getoct(const char *buf, uint32_t *N){ + const char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '7'){ + break; + } + if(num & 0xe0000000){ // overflow + *N = 0xffffff; + return start; + } + num <<= 3; + num += c - '0'; + ++buf; + } + *N = num; + return buf; +} +// read binary number (without b prefix!) +static const char *getbin(const char *buf, uint32_t *N){ + const char *start = (char*)buf; + uint32_t num = 0; + while(*buf){ + char c = *buf; + if(c < '0' || c > '1'){ + break; + } + if(num & 0x80000000){ // overflow + *N = 0xffffff; + return start; + } + 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 or if *N==0xffffffff there was overflow) + */ +const char *getnum(const char *txt, uint32_t *N){ + const char *nxt = NULL; + const char *s = omit_spaces(txt); + if(*s == '0'){ // hex, oct or 0 + if(s[1] == 'x' || s[1] == 'X'){ // hex + nxt = gethex(s+2, N); + if(nxt == s+2) nxt = (char*)txt; + }else if(s[1] > '0'-1 && s[1] < '8'){ // oct + nxt = getoct(s+1, N); + if(nxt == s+1) nxt = (char*)txt; + }else{ // 0 + nxt = s+1; + *N = 0; + } + }else if(*s == 'b' || *s == 'B'){ + nxt = getbin(s+1, N); + if(nxt == s+1) nxt = (char*)txt; + }else{ + nxt = getdec(s, N); + if(nxt == s) nxt = (char*)txt; + } + return nxt; +} + +// get signed integer +const char *getint(const char *txt, int32_t *I){ + const char *s = omit_spaces(txt); + int32_t sign = 1; + uint32_t U; + if(*s == '-'){ + sign = -1; + ++s; + } + const char *nxt = getnum(s, &U); + if(nxt == s) return txt; + if(U & 0x80000000) return txt; // overfull + *I = sign * (int32_t)U; + return nxt; +} diff --git a/F3:F303/MLX90640/strfunc.h b/F3:F303/MLX90640/strfunc.h new file mode 100644 index 0000000..6161bfd --- /dev/null +++ b/F3:F303/MLX90640/strfunc.h @@ -0,0 +1,32 @@ +/* + * This file is part of the mlx90640 project. + * Copyright 2025 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 + +#include +#include + +void u16s(uint16_t n, char *buf); +void hexdump16(int (*sendfun)(const char *s), uint16_t *arr, uint16_t len); +void hexdump(int (*sendfun)(const char *s), uint8_t *arr, uint16_t len); +const char *u2str(uint32_t val); +const char *i2str(int32_t i); +const char *uhex2str(uint32_t val); +const char *getnum(const char *txt, uint32_t *N); +char *omit_spaces(const char *buf); +const char *getint(const char *txt, int32_t *I); diff --git a/F3:F303/MLX90640/usb_descr.c b/F3:F303/MLX90640/usb_descr.c new file mode 100644 index 0000000..98bf896 --- /dev/null +++ b/F3:F303/MLX90640/usb_descr.c @@ -0,0 +1,210 @@ +/* + * Copyright 2024 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 "usb_descr.h" + +// low/high for uint16_t +#define L16(x) (x & 0xff) +#define H16(x) (x >> 8) + +static const uint8_t USB_DeviceDescriptor[] = { + USB_DT_DEVICE_SIZE, // bLength + USB_DT_DEVICE, // bDescriptorType + L16(bcdUSB), // bcdUSB_L + H16(bcdUSB), // bcdUSB_H + USB_CLASS_MISC, // bDeviceClass + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0BUFSZ, // bMaxPacketSize + L16(idVendor), // idVendor_L + H16(idVendor), // idVendor_H + L16(idProduct), // idProduct_L + H16(idProduct), // idProduct_H + L16(bcdDevice_Ver), // bcdDevice_Ver_L + H16(bcdDevice_Ver), // bcdDevice_Ver_H + iMANUFACTURER_DESCR, // iManufacturer - indexes of string descriptors in array + iPRODUCT_DESCR, // iProduct + iSERIAL_DESCR, // iSerial + bNumConfigurations // bNumConfigurations +}; + +static const uint8_t USB_DeviceQualifierDescriptor[] = { + USB_DT_QUALIFIER_SIZE, //bLength + USB_DT_QUALIFIER, // bDescriptorType + L16(bcdUSB), // bcdUSB_L + H16(bcdUSB), // bcdUSB_H + USB_CLASS_PER_INTERFACE, // bDeviceClass + bDeviceSubClass, // bDeviceSubClass + bDeviceProtocol, // bDeviceProtocol + USB_EP0BUFSZ, // bMaxPacketSize0 + bNumConfigurations, // bNumConfigurations + 0 // Reserved +}; + +#define wTotalLength (USB_DT_CONFIG_SIZE + (bNumInterfaces * USB_DT_INTERFACE_SIZE) + (bTotNumEndpoints * USB_DT_ENDPOINT_SIZE) + (bNumCsInterfaces * USB_DT_CS_INTERFACE_SIZE) - 1) + +static const uint8_t USB_ConfigDescriptor[] = { + // Configuration Descriptor + USB_DT_CONFIG_SIZE, // bLength: Configuration Descriptor size + USB_DT_CONFIG, // bDescriptorType: Configuration + L16(wTotalLength), // wTotalLength.L :no of returned bytes + H16(wTotalLength), // wTotalLength.H + bNumInterfaces, // bNumInterfaces + 1, // bConfigurationValue: Current configuration value + 0, // iConfiguration: Index of string descriptor describing the configuration or 0 + BusPowered, // bmAttributes - Bus powered + 50, // MaxPower in 2mA units + //--------------------------------------------------------------------------- + // Virtual command Interface Descriptor + USB_DT_INTERFACE_SIZE, // bLength: Interface Descriptor size + USB_DT_INTERFACE, // bDescriptorType: Interface + 0, // bInterfaceNumber: Number of Interface + 0, // bAlternateSetting: Alternate setting + 1, // bNumEndpoints: one for this + USB_CLASS_COMM, // bInterfaceClass + 2, // bInterfaceSubClass: ACM + 1, // bInterfaceProtocol: Common AT commands + iINTERFACE_DESCR1, // iInterface + // ---- CS Interfaces + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 0, // bDescriptorSubtype: Header Func Desc + 0x10, // bcdCDC: spec release number + 1, // bDataInterface + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 1, // bDescriptorSubtype: Call Management Func Desc + 0, // bmCapabilities: D0+D1 + 1, // bDataInterface + USB_DT_CS_INTERFACE_SIZE-1, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 2, // bDescriptorSubtype: Abstract Control Management desc + 2, // bmCapabilities + USB_DT_CS_INTERFACE_SIZE, // bLength + USB_DT_CS_INTERFACE, // bDescriptorType: CS_INTERFACE + 6, // bDescriptorSubtype: Union func desc + 0, // bMasterInterface: Communication class interface + 1, // bSlaveInterface0: Data Class Interface + // Virtual endpoint 1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x8A, // bEndpointAddress IN10 + USB_BM_ATTR_INTERRUPT, // bmAttributes: Interrupt + L16(USB_EP1BUFSZ), // wMaxPacketSize LO + H16(USB_EP1BUFSZ), // wMaxPacketSize HI + 0x10, // bInterval: 16ms + //--------------------------------------------------------------------------- + // Data interface + USB_DT_INTERFACE_SIZE, // bLength: Interface Descriptor size + USB_DT_INTERFACE, // bDescriptorType: Interface + 1, // bInterfaceNumber: Number of Interface + 0, // bAlternateSetting: Alternate setting + 2, // bNumEndpoints: in and out + USB_CLASS_DATA, // bInterfaceClass + 2, // bInterfaceSubClass: ACM + 0, // bInterfaceProtocol + 0, // iInterface + //Endpoint IN1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x81, // bEndpointAddress: IN1 + USB_BM_ATTR_BULK, // bmAttributes: Bulk + L16(USB_TXBUFSZ), // wMaxPacketSize LO + H16(USB_TXBUFSZ), // wMaxPacketSize HI + 0, // bInterval: ignore for Bulk transfer + // Endpoint OUT1 Descriptor + USB_DT_ENDPOINT_SIZE, // bLength: Endpoint Descriptor size + USB_DT_ENDPOINT, // bDescriptorType: Endpoint + 0x01, // bEndpointAddress: OUT1 + USB_BM_ATTR_BULK, // bmAttributes: Bulk + L16(USB_RXBUFSZ), // wMaxPacketSize LO + H16(USB_RXBUFSZ), // wMaxPacketSize HI + 0, // bInterval: ignore for Bulk transfer + +}; + +//const uint8_t HID_ReportDescriptor[]; + +_USB_LANG_ID_(LD, LANG_US); +_USB_STRING_(SD, u"0.0.1"); +_USB_STRING_(MD, u"eddy@sao.ru"); +_USB_STRING_(PD, u"MLX90640 sensor management"); +_USB_STRING_(ID, u"mlx_sensor"); + +static const void* const StringDescriptor[iDESCR_AMOUNT] = { + [iLANGUAGE_DESCR] = &LD, + [iMANUFACTURER_DESCR] = &MD, + [iPRODUCT_DESCR] = &PD, + [iSERIAL_DESCR] = &SD, + [iINTERFACE_DESCR1] = &ID +}; + +static void wr0(const uint8_t *buf, uint16_t size, uint16_t askedsize){ + if(askedsize < size) size = askedsize; // shortened request + if(size < USB_EP0BUFSZ){ + EP_WriteIRQ(0, buf, size); + return; + } + while(size){ + uint16_t l = size; + if(l > USB_EP0BUFSZ) l = USB_EP0BUFSZ; + EP_WriteIRQ(0, buf, l); + buf += l; + size -= l; + uint8_t needzlp = (l == USB_EP0BUFSZ) ? 1 : 0; + if(size || needzlp){ // send last data buffer + uint16_t epstatus = KEEP_DTOG(USB->EPnR[0]); + // keep DTOGs, clear CTR_RX,TX, set TX VALID, leave stat_Rx + USB->EPnR[0] = (epstatus & ~(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, NULL, 0); + } + } +} + +void get_descriptor(config_pack_t *pack){ + uint8_t descrtype = pack->wValue >> 8, + descridx = pack->wValue & 0xff; + switch(descrtype){ + case DEVICE_DESCRIPTOR: + wr0(USB_DeviceDescriptor, sizeof(USB_DeviceDescriptor), pack->wLength); + break; + case CONFIGURATION_DESCRIPTOR: + wr0(USB_ConfigDescriptor, sizeof(USB_ConfigDescriptor), pack->wLength); + break; + case STRING_DESCRIPTOR: + if(descridx < iDESCR_AMOUNT){ + wr0((const uint8_t *)StringDescriptor[descridx], *((uint8_t*)StringDescriptor[descridx]), pack->wLength); + }else{ + EP_WriteIRQ(0, NULL, 0); + } + break; + case DEVICE_QUALIFIER_DESCRIPTOR: + wr0(USB_DeviceQualifierDescriptor, sizeof(USB_DeviceQualifierDescriptor), pack->wLength); + break; + /* case HID_REPORT_DESCRIPTOR: + wr0(HID_ReportDescriptor, sizeof(HID_ReportDescriptor), pack->wLength); + break;*/ + default: + break; + } +} diff --git a/F3:F303/MLX90640/usb_descr.h b/F3:F303/MLX90640/usb_descr.h new file mode 100644 index 0000000..d260af2 --- /dev/null +++ b/F3:F303/MLX90640/usb_descr.h @@ -0,0 +1,62 @@ +/* + * Copyright 2024 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 + +#include + +#include "usb_lib.h" + +// definition of parts common for USB_DeviceDescriptor & USB_DeviceQualifierDescriptor +// bcdUSB: 1.10 +#define bcdUSB 0x0110 +// Class - Misc (EF), subclass - common (2), protocol - interface association descr (1) +#define bDeviceSubClass 0x02 +#define bDeviceProtocol 0x01 +#define idVendor 0x0483 +#define idProduct 0x5740 +#define bcdDevice_Ver 0x0200 +#define bNumConfigurations 1 + +// amount of interfaces and endpoints (except 0) used +#define bNumInterfaces 2 +#define bTotNumEndpoints 3 +#define bNumCsInterfaces 4 + +// powered +#define BusPowered (1<<7) +#define SelfPowered (1<<6) +#define RemoteWakeup (1<<5) + +// buffer sizes +// for USB FS EP0 buffers are from 8 to 64 bytes long +#define USB_EP0BUFSZ 64 +#define USB_EP1BUFSZ 10 +// Rx/Tx EPs +#define USB_RXBUFSZ 64 +#define USB_TXBUFSZ 64 + +// string descriptors +enum{ + iLANGUAGE_DESCR, + iMANUFACTURER_DESCR, + iPRODUCT_DESCR, + iSERIAL_DESCR, + iINTERFACE_DESCR1, + iDESCR_AMOUNT +}; + +void get_descriptor(config_pack_t *pack); diff --git a/F3:F303/MLX90640/usb_dev.c b/F3:F303/MLX90640/usb_dev.c new file mode 100644 index 0000000..8e519e4 --- /dev/null +++ b/F3:F303/MLX90640/usb_dev.c @@ -0,0 +1,240 @@ +/* + * Copyright 2024 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 + +#include "ringbuffer.h" +#include "usb_descr.h" +#include "usb_dev.h" + +// 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 + +// inbuf overflow when receiving +static volatile uint8_t bufovrfl = 0; + +// receive buffer: hold data until chkin() call +static uint8_t volatile rcvbuf[USB_RXBUFSZ]; +static uint8_t volatile rcvbuflen = 0; +// line coding +usb_LineCoding WEAK lineCoding = {115200, 0, 0, 8}; +// CDC configured and ready to use +volatile uint8_t CDCready = 0; + +// ring buffers for incoming and outgoing data +static uint8_t obuf[RBOUTSZ], ibuf[RBINSZ]; +static volatile ringbuffer rbout = {.data = obuf, .length = RBOUTSZ, .head = 0, .tail = 0}; +static volatile ringbuffer rbin = {.data = ibuf, .length = RBINSZ, .head = 0, .tail = 0}; +// last send data size +static volatile int lastdsz = 0; + +static void chkin(){ + if(bufovrfl) return; // allow user to know that previous buffer was overflowed and cleared + if(!rcvbuflen) return; + int w = RB_write((ringbuffer*)&rbin, (uint8_t*)rcvbuf, rcvbuflen); + if(w < 0){ + return; + } + if(w != rcvbuflen) bufovrfl = 1; + rcvbuflen = 0; + uint16_t status = KEEP_DTOG(USB->EPnR[1]); // don't change DTOG + USB->EPnR[1] = (status & ~(USB_EPnR_STAT_TX|USB_EPnR_CTR_RX)) ^ USB_EPnR_STAT_RX; // prepare to get next data portion +} + +// called from transmit EP to send next data portion or by user - when new transmission starts +static void send_next(){ + uint8_t usbbuff[USB_TXBUFSZ]; + int buflen = RB_read((ringbuffer*)&rbout, (uint8_t*)usbbuff, USB_TXBUFSZ); + if(buflen == 0){ + if(lastdsz == 64) EP_Write(1, NULL, 0); // send ZLP after 64 bits packet when nothing more to send + lastdsz = 0; + return; + }else if(buflen < 0){ + lastdsz = 0; + return; + } + EP_Write(1, (uint8_t*)usbbuff, buflen); + lastdsz = buflen; +} + +// data IN/OUT handler +static void rxtx_handler(){ + uint16_t epstatus = KEEP_DTOG(USB->EPnR[1]); + if(RX_FLAG(epstatus)){ // receive data + if(rcvbuflen){ + bufovrfl = 1; // lost last data + rcvbuflen = 0; + } + rcvbuflen = EP_Read(1, (uint8_t*)rcvbuf); + USB->EPnR[1] = epstatus & ~(USB_EPnR_CTR_RX | USB_EPnR_STAT_RX | USB_EPnR_STAT_TX); // keep RX in STALL state until read data + chkin(); // try to write current data into RXbuf if it's not busy + }else{ // tx successfull + USB->EPnR[1] = (epstatus & ~(USB_EPnR_CTR_TX | USB_EPnR_STAT_TX)) ^ USB_EPnR_STAT_RX; + send_next(); + } +} + +// weak handlers: change them somewhere else if you want to setup USART +// SET_LINE_CODING +void WEAK linecoding_handler(usb_LineCoding *lc){ + lineCoding = *lc; +} + +// SET_CONTROL_LINE_STATE +void WEAK clstate_handler(uint16_t val){ + CDCready = val; // CONTROL_DTR | CONTROL_RTS -> interface connected; 0 -> disconnected +} + +// SEND_BREAK +void WEAK break_handler(){ + CDCready = 0; +} + + +// USB is configured: setup endpoints +void set_configuration(){ + EP_Init(1, EP_TYPE_BULK, USB_TXBUFSZ, USB_RXBUFSZ, rxtx_handler); // IN1 and OUT1 +} + +// PL2303 CLASS request +void usb_class_request(config_pack_t *req, uint8_t *data, uint16_t datalen){ + uint8_t recipient = REQUEST_RECIPIENT(req->bmRequestType); + uint8_t dev2host = (req->bmRequestType & 0x80) ? 1 : 0; + switch(recipient){ + case REQ_RECIPIENT_INTERFACE: + switch(req->bRequest){ + case SET_LINE_CODING: + if(!data || !datalen) break; // wait for data + if(datalen == sizeof(usb_LineCoding)) + linecoding_handler((usb_LineCoding*)data); + break; + case GET_LINE_CODING: + EP_WriteIRQ(0, (uint8_t*)&lineCoding, sizeof(lineCoding)); + break; + case SET_CONTROL_LINE_STATE: + clstate_handler(req->wValue); + break; + case SEND_BREAK: + break_handler(); + break; + default: + break; + } + break; + default: + if(dev2host) EP_WriteIRQ(0, NULL, 0); + } + if(!dev2host) EP_WriteIRQ(0, NULL, 0); +} + +// blocking send full content of ring buffer +int USB_sendall(){ + while(lastdsz > 0){ + if(!CDCready) return FALSE; + } + return TRUE; +} + +// put `buf` into queue to send +int USB_send(const uint8_t *buf, int len){ + if(!buf || !CDCready || !len) return FALSE; + while(len){ + int a = RB_write((ringbuffer*)&rbout, buf, len); + if(a > 0){ + len -= a; + buf += a; + } else if (a < 0) continue; // do nothing if buffer is in reading state + if(lastdsz == 0) send_next(); // need to run manually - all data sent, so no IRQ on IN + } + return TRUE; +} + +int USB_putbyte(uint8_t byte){ + if(!CDCready) return FALSE; + int l = 0; + while((l = RB_write((ringbuffer*)&rbout, &byte, 1)) != 1){ + if(l < 0) continue; + } + if(lastdsz == 0) send_next(); // need to run manually - all data sent, so no IRQ on IN + return TRUE; +} + +int USB_sendstr(const char *string){ + if(!string || !CDCready) return FALSE; + int len = 0; + const char *b = string; + while(*b++) ++len; + if(!len) return FALSE; + return USB_send((const uint8_t*)string, len); +} + +/** + * @brief USB_receive - get binary data from receiving ring-buffer + * @param buf (i) - buffer for received data + * @param len - length of `buf` + * @return amount of received bytes (negative, if overfull happened) + */ +int USB_receive(uint8_t *buf, int len){ + chkin(); + if(bufovrfl){ + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + bufovrfl = 0; + return -1; + } + int sz = RB_read((ringbuffer*)&rbin, buf, len); + if(sz < 0) return 0; // buffer in writting state + return sz; +} + +/** + * @brief USB_receivestr - get string up to '\n' and replace '\n' with 0 + * @param buf - receiving buffer + * @param len - its length + * @return strlen or negative value indicating overflow (if so, string won't be ends with 0 and buffer should be cleared) + */ +int USB_receivestr(char *buf, int len){ + chkin(); + if(bufovrfl){ + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + bufovrfl = 0; + return -1; + } + int l = RB_readto((ringbuffer*)&rbin, '\n', (uint8_t*)buf, len); + if(l < 1){ + if(rbin.length == RB_datalen((ringbuffer*)&rbin)){ // buffer is full but no '\n' found + while(1 != RB_clearbuf((ringbuffer*)&rbin)); + return -1; + } + return 0; + } + if(l == 0) return 0; + buf[l-1] = 0; // replace '\n' with strend + return l; +} + diff --git a/F3:F303/MLX90640/usb_dev.h b/F3:F303/MLX90640/usb_dev.h new file mode 100644 index 0000000..f787b12 --- /dev/null +++ b/F3:F303/MLX90640/usb_dev.h @@ -0,0 +1,58 @@ +/* + * Copyright 2024 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 + +#include +#include "usb_lib.h" + +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; + +extern usb_LineCoding lineCoding; +extern volatile uint8_t CDCready; + +void break_handler(); +void clstate_handler(uint16_t val); +void linecoding_handler(usb_LineCoding *lc); + + +// sizes of ringbuffers for outgoing and incoming data +#define RBOUTSZ (1024) +#define RBINSZ (1024) + +#define newline() USB_putbyte('\n') +#define USND(s) do{USB_sendstr(s); USB_putbyte('\n');}while(0) +#define U(s) USB_sendstr(s) + +int USB_sendall(); +int USB_send(const uint8_t *buf, int len); +int USB_putbyte(uint8_t byte); +int USB_sendstr(const char *string); +int USB_receive(uint8_t *buf, int len); +int USB_receivestr(char *buf, int len); diff --git a/F3:F303/MLX90640/usb_lib.c b/F3:F303/MLX90640/usb_lib.c new file mode 100644 index 0000000..e390b20 --- /dev/null +++ b/F3:F303/MLX90640/usb_lib.c @@ -0,0 +1,368 @@ +/* + * Copyright 2024 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 + +#include "usb_lib.h" +#include "usb_descr.h" +#include "usb_dev.h" + +static ep_t endpoints[STM32ENDPOINTS]; + +static uint16_t USB_Addr = 0; +static uint8_t setupdatabuf[EP0DATABUF_SIZE]; +static config_pack_t *setup_packet = (config_pack_t*) setupdatabuf; +volatile uint8_t usbON = 0; // device is configured and active + +static uint16_t configuration = 0; // reply for GET_CONFIGURATION (==1 if configured) +static inline void std_d2h_req(){ + uint16_t st = 0; + switch(setup_packet->bRequest){ + case GET_DESCRIPTOR: + get_descriptor(setup_packet); + break; + case GET_STATUS: + EP_WriteIRQ(0, (uint8_t *)&st, 2); // send status: Bus Powered + break; + case GET_CONFIGURATION: + EP_WriteIRQ(0, (uint8_t*)&configuration, 1); + break; + default: + EP_WriteIRQ(0, NULL, 0); + 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_Addr = setup_packet->wValue; + break; + case SET_CONFIGURATION: + // Now device configured + configuration = setup_packet->wValue; + set_configuration(); + usbON = 1; + break; + default: + break; + } +} + +void WEAK usb_standard_request(){ + uint8_t recipient = REQUEST_RECIPIENT(setup_packet->bmRequestType); + uint8_t dev2host = (setup_packet->bmRequestType & 0x80) ? 1 : 0; + switch(recipient){ + case REQ_RECIPIENT_DEVICE: + if(dev2host){ + std_d2h_req(); + }else{ + std_h2d_req(); + } + break; + case REQ_RECIPIENT_INTERFACE: + if(dev2host && setup_packet->bRequest == GET_DESCRIPTOR){ + get_descriptor(setup_packet); + } + break; + case REQ_RECIPIENT_ENDPOINT: + if(setup_packet->bRequest == CLEAR_FEATURE){ + }else{ /* wrong */ } + break; + default: + break; + } + if(!dev2host) EP_WriteIRQ(0, NULL, 0); +} + +void WEAK usb_class_request(config_pack_t *req, uint8_t _U_ *data, uint16_t _U_ datalen){ + switch(req->bRequest){ + case GET_INTERFACE: + break; + case SET_CONFIGURATION: // set featuring by req->wValue + break; + default: + break; + } + if(0 == (setup_packet->bmRequestType & 0x80)) // host2dev + EP_WriteIRQ(0, NULL, 0); +} + +void WEAK usb_vendor_request(config_pack_t _U_ *packet, uint8_t _U_ *data, uint16_t _U_ datalen){ + if(0 == (setup_packet->bmRequestType & 0x80)) // host2dev + EP_WriteIRQ(0, NULL, 0); +} + +/* +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(){ + uint8_t ep0dbuflen = 0; + uint8_t ep0databuf[EP0DATABUF_SIZE]; + uint16_t epstatus = KEEP_DTOG(USB->EPnR[0]); // EP0R on input -> return this value after modifications + int rxflag = RX_FLAG(epstatus); + //if(rxflag){ } + // 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(epstatus & USB_EPnR_SETUP){ // setup packet -> copy data to conf_pack + EP_Read(0, setupdatabuf); + // interrupt handler will be called later + }else if(epstatus & USB_EPnR_CTR_RX){ // data packet -> push received data to ep0databuf + //if(endpoints[0].rx_cnt){ } + ep0dbuflen = EP_Read(0, ep0databuf); + } + } + if(rxflag){ + uint8_t reqtype = REQUEST_TYPE(setup_packet->bmRequestType); + switch(reqtype){ + case REQ_TYPE_STANDARD: + if(SETUP_FLAG(epstatus)){ + usb_standard_request(); + }else{ } + break; + case REQ_TYPE_CLASS: + usb_class_request(setup_packet, ep0databuf, ep0dbuflen); + break; + case REQ_TYPE_VENDOR: + usb_vendor_request(setup_packet, ep0databuf, ep0dbuflen); + break; + default: + EP_WriteIRQ(0, NULL, 0); + break; + } + } + if(TX_FLAG(epstatus)){ + // now we can change address after enumeration + if ((USB->DADDR & USB_DADDR_ADD) != USB_Addr){ + USB->DADDR = USB_DADDR_EF | USB_Addr; + usbON = 0; + } + } + //epstatus = KEEP_DTOG(USB->EPnR[0]); + if(rxflag) epstatus ^= USB_EPnR_STAT_TX; // start ZLP or 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; +} + +/** + * 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){ + if(size > endpoints[number].txbufsz) size = endpoints[number].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; +#if defined USB1_16 + // very bad: what if `size` is odd? + uint32_t *out = (uint32_t *)endpoints[number].tx_buf; + for(int i = 0; i < N2; ++i, ++out){ + *out = buf16[i]; + } +#elif defined USB2_16 + // use mememcpy instead? + for(int i = 0; i < N2; i++){ + endpoints[number].tx_buf[i] = buf16[i]; + } +#else +#error "Define USB1_16 or USB2_16" +#endif + 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 epstatus = KEEP_DTOG(USB->EPnR[number]); + // keep DTOGs and RX stat, clear CTR_TX & set TX VALID to start transmission + USB->EPnR[number] = (epstatus & ~(USB_EPnR_CTR_TX | USB_EPnR_STAT_RX)) ^ 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 sz = endpoints[number].rx_cnt; + if(!sz) return 0; + endpoints[number].rx_cnt = 0; +#if defined USB1_16 + int n = (sz + 1) >> 1; + uint32_t *in = (uint32_t*)endpoints[number].rx_buf; + uint16_t *out = (uint16_t*)buf; + for(int i = 0; i < n; ++i, ++in) + out[i] = *(uint16_t*)in; +#elif defined USB2_16 + // use mememcpy instead? + for(int i = 0; i < sz; ++i) + buf[i] = endpoints[number].rx_buf[i]; +#else +#error "Define USB1_16 or USB2_16" +#endif + return sz; +} + + +static uint16_t lastaddr = LASTADDR_DEFAULT; +/** + * Endpoint initialisation + * @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)(ep_t ep)){ + if(number >= STM32ENDPOINTS) return 4; // out of configured amount + if(txsz > USB_BTABLE_SIZE/ACCESSZ || rxsz > USB_BTABLE_SIZE/ACCESSZ) return 1; // buffer too large + if(lastaddr + txsz + rxsz >= USB_BTABLE_SIZE/ACCESSZ) 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) 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 * ACCESSZ); + 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 * ACCESSZ); + lastaddr += rxsz; + USB_BTABLE->EP[number].USB_COUNT_RX = countrx << 10; + endpoints[number].func = func; + return 0; +} + +// standard IRQ handler +void USB_IRQ(){ + uint32_t CNTR = USB->CNTR; + USB->CNTR = 0; + if(USB->ISTR & USB_ISTR_RESET){ + usbON = 0; + // Reinit registers + CNTR = USB_CNTR_RESETM | USB_CNTR_CTRM | USB_CNTR_SUSPM; + // Endpoint 0 - CONTROL + // ON USB LS size of EP0 may be 8 bytes, but on FS it should be 64 bytes! + lastaddr = LASTADDR_DEFAULT; + // clear address, leave only enable bit + USB->DADDR = USB_DADDR_EF; + USB->ISTR = ~USB_ISTR_RESET; + if(EP_Init(0, EP_TYPE_CONTROL, USB_EP0BUFSZ, USB_EP0BUFSZ, EP0_Handler)){ + return; + }; + } + if(USB->ISTR & USB_ISTR_CTR){ + // EP number + uint8_t n = USB->ISTR & USB_ISTR_EPID; + // copy received bytes amount + endpoints[n].rx_cnt = USB_BTABLE->EP[n].USB_COUNT_RX & 0x3FF; // low 10 bits is counter + // call EP handler + if(endpoints[n].func) endpoints[n].func(); + } + if(USB->ISTR & USB_ISTR_WKUP){ // wakeup +#ifndef STM32F0 + CNTR &= ~(USB_CNTR_FSUSP | USB_CNTR_LP_MODE | USB_CNTR_WKUPM); // clear suspend flags +#else + CNTR &= ~(USB_CNTR_FSUSP | USB_CNTR_LPMODE | USB_CNTR_WKUPM); +#endif + USB->ISTR = ~USB_ISTR_WKUP; + } + if(USB->ISTR & USB_ISTR_SUSP){ // suspend -> still no connection, may sleep + usbON = 0; +#ifndef STM32F0 + CNTR |= USB_CNTR_FSUSP | USB_CNTR_LP_MODE | USB_CNTR_WKUPM; +#else + CNTR |= USB_CNTR_FSUSP | USB_CNTR_LPMODE | USB_CNTR_WKUPM; +#endif + CNTR &= ~(USB_CNTR_SUSPM); + USB->ISTR = ~USB_ISTR_SUSP; + } + USB->CNTR = CNTR; // rewoke interrupts +} + +// here we suppose that all PIN settings done in hw_setup earlier +void USB_setup(){ +#if defined STM32F3 + NVIC_DisableIRQ(USB_LP_IRQn); + // remap USB LP & Wakeup interrupts to 75 and 76 - works only on pure F303 + RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; // enable tacting of SYSCFG + SYSCFG->CFGR1 |= SYSCFG_CFGR1_USB_IT_RMP; +#elif defined STM32F1 + NVIC_DisableIRQ(USB_LP_CAN1_RX0_IRQn); + NVIC_DisableIRQ(USB_HP_CAN1_TX_IRQn); +#elif defined STM32F0 + NVIC_DisableIRQ(USB_IRQn); + RCC->APB1ENR |= RCC_APB1ENR_CRSEN; + 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; +#endif + RCC->APB1ENR |= RCC_APB1ENR_USBEN; + //?? + USB->CNTR = USB_CNTR_FRES; // Force USB Reset + for(uint32_t ctr = 0; ctr < 72000; ++ctr) nop(); // wait >1ms + USB->CNTR = 0; + USB->BTABLE = 0; + USB->DADDR = 0; + USB->ISTR = 0; + USB->CNTR = USB_CNTR_RESETM; // allow only reset interrupts +#if defined STM32F3 + NVIC_EnableIRQ(USB_LP_IRQn); +#elif defined STM32F1 + NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn); +#elif defined STM32F0 + USB->BCDR |= USB_BCDR_DPPU; + NVIC_EnableIRQ(USB_IRQn); +#endif +} + + +#if defined STM32F3 +void usb_lp_isr() __attribute__ ((alias ("USB_IRQ"))); +#elif defined STM32F1 +void usb_lp_can_rx0_isr() __attribute__ ((alias ("USB_IRQ"))); +#elif defined STM32F0 +void usb_isr() __attribute__ ((alias ("USB_IRQ"))); +#endif diff --git a/F3:F303/MLX90640/usb_lib.h b/F3:F303/MLX90640/usb_lib.h new file mode 100644 index 0000000..d55c83f --- /dev/null +++ b/F3:F303/MLX90640/usb_lib.h @@ -0,0 +1,328 @@ +/* + * Copyright 2024 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 + +#include +#include + +#ifndef _U_ +#define _U_ __attribute__((unused)) +#endif + +/****************************************************************** + * Hardware registers etc * + *****************************************************************/ +#if defined STM32F0 +#include +#elif defined STM32F1 +#include +// there's no this define in standard header +#define USB_BASE ((uint32_t)0x40005C00) +#elif defined STM32F3 +#include +#endif + +// max endpoints number +#define STM32ENDPOINTS 8 +/** + * Buffers size definition + **/ + +// F0 - USB2_16; F1 - USB1_16; F3 - 1/2 depending on series +#if !defined USB1_16 && !defined USB2_16 +#if defined STM32F0 +#define USB2_16 +#elif defined STM32F1 +#define USB1_16 +#else +#error "Can't determine USB1_16 or USB2_16, define by hands" +#endif +#endif + +// BTABLE_SIZE FOR STM32F3: +// In STM32F303/302xB/C, 512 bytes SRAM is not shared with CAN. +// In STM32F302x6/x8 and STM32F30xxD/E, 726 bytes dedicated SRAM and 256 bytes shared SRAM with CAN i.e. +// 1Kbytes dedicated SRAM in case CAN is disabled. +// remember, that USB_BTABLE_SIZE will be divided by ACCESSZ, so don't divide it twice for 32-bit addressing + +#ifdef NOCAN +#if defined STM32F0 +#define USB_BTABLE_SIZE 1024 +#elif defined STM32F3 +#define USB_BTABLE_SIZE 726 +//#warning "Please, check real buffer size due to docs" +#else +#error "define STM32F0 or STM32F3" +#endif +#else // !NOCAN: F0/F3 with CAN or F1 (can't simultaneously run CAN and USB) +#if defined STM32F0 +#define USB_BTABLE_SIZE 768 +#elif defined STM32F3 +#define USB_BTABLE_SIZE 726 +//#warning "Please, check real buffer size due to docs" +#else // STM32F103: 1024 bytes but with 32-bit addressing +#define USB_BTABLE_SIZE 1024 +#endif +#endif // NOCAN + +// first 64 bytes of USB_BTABLE are registers! + +#define USB_BTABLE_BASE 0x40006000 +#define USB ((USB_TypeDef *) USB_BASE) + +#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; +#ifdef STM32F0 + __IO uint32_t LPMCSR; + __IO uint32_t BCDR; +#endif +} USB_TypeDef; + +// F303 D/E have 2x16 access scheme +typedef struct{ +#if defined USB2_16 + __IO uint16_t USB_ADDR_TX; + __IO uint16_t USB_COUNT_TX; + __IO uint16_t USB_ADDR_RX; + __IO uint16_t USB_COUNT_RX; +#define ACCESSZ (1) +#define BUFTYPE uint8_t +#elif defined USB1_16 + __IO uint32_t USB_ADDR_TX; + __IO uint32_t USB_COUNT_TX; + __IO uint32_t USB_ADDR_RX; + __IO uint32_t USB_COUNT_RX; +#define ACCESSZ (2) +#define BUFTYPE uint16_t +#else +#error "Define USB1_16 or USB2_16" +#endif +} USB_EPDATA_TypeDef; + + +typedef struct{ + __IO USB_EPDATA_TypeDef EP[STM32ENDPOINTS]; +} USB_BtableDef; + +#define EP0DATABUF_SIZE (64) +#define LASTADDR_DEFAULT (STM32ENDPOINTS * 8) + +/****************************************************************** + * Defines from usb.h * + *****************************************************************/ + +/* + * Device and/or Interface Class codes + */ +#define USB_CLASS_PER_INTERFACE 0 +#define USB_CLASS_AUDIO 1 +#define USB_CLASS_COMM 2 +#define USB_CLASS_HID 3 +#define USB_CLASS_PRINTER 7 +#define USB_CLASS_PTP 6 +#define USB_CLASS_MASS_STORAGE 8 +#define USB_CLASS_HUB 9 +#define USB_CLASS_DATA 10 +#define USB_CLASS_MISC 0xef +#define USB_CLASS_VENDOR_SPEC 0xff + +/* + * Descriptor types + */ +#define USB_DT_DEVICE 0x01 +#define USB_DT_CONFIG 0x02 +#define USB_DT_STRING 0x03 +#define USB_DT_INTERFACE 0x04 +#define USB_DT_ENDPOINT 0x05 +#define USB_DT_QUALIFIER 0x06 +#define USB_DT_IAD 0x0B + +#define USB_DT_HID 0x21 +#define USB_DT_REPORT 0x22 +#define USB_DT_PHYSICAL 0x23 +#define USB_DT_CS_INTERFACE 0x24 +#define USB_DT_HUB 0x29 + +/* + * Descriptor sizes per descriptor type + */ +#define USB_DT_DEVICE_SIZE 18 +#define USB_DT_CONFIG_SIZE 9 +#define USB_DT_INTERFACE_SIZE 9 +#define USB_DT_HID_SIZE 9 +#define USB_DT_ENDPOINT_SIZE 7 +#define USB_DT_QUALIFIER_SIZE 10 +#define USB_DT_CS_INTERFACE_SIZE 5 +#define USB_DT_IAD_SIZE 8 + + +// bmRequestType & 0x80 == dev2host (1) or host2dev (0) +// recipient: bmRequestType & 0x1f +#define REQUEST_RECIPIENT(b) (b & 0x1f) +#define REQ_RECIPIENT_DEVICE 0 +#define REQ_RECIPIENT_INTERFACE 1 +#define REQ_RECIPIENT_ENDPOINT 2 +#define REQ_RECIPIENT_OTHER 3 +// type: [bmRequestType & 0x60 >> 5] +#define REQUEST_TYPE(b) ((b&0x60)>>5) +#define REQ_TYPE_STANDARD 0 +#define REQ_TYPE_CLASS 1 +#define REQ_TYPE_VENDOR 2 +#define REQ_TYPE_RESERVED 3 + + +//#define VENDOR_REQUEST 0x01 + +// standard device requests +#define GET_STATUS 0x00 +#define CLEAR_FEATURE 0x01 +#define SET_FEATURE 0x03 +#define SET_ADDRESS 0x05 +#define GET_DESCRIPTOR 0x06 +#define SET_DESCRIPTOR 0x07 +#define GET_CONFIGURATION 0x08 +#define SET_CONFIGURATION 0x09 +// and some standard interface requests +#define GET_INTERFACE 0x0A +#define SET_INTERFACE 0x0B +// and some standard endpoint requests +#define SYNC_FRAME 0x0C + +// Types of descriptors +#define DEVICE_DESCRIPTOR 0x01 +#define CONFIGURATION_DESCRIPTOR 0x02 +#define STRING_DESCRIPTOR 0x03 +#define DEVICE_QUALIFIER_DESCRIPTOR 0x06 +#define DEBUG_DESCRIPTOR 0x0a +#define HID_REPORT_DESCRIPTOR 0x22 + +// EP types for EP_init +#define EP_TYPE_BULK 0x00 +#define EP_TYPE_CONTROL 0x01 +#define EP_TYPE_ISO 0x02 +#define EP_TYPE_INTERRUPT 0x03 + +// EP types for descriptors +#define USB_BM_ATTR_CONTROL 0x00 +#define USB_BM_ATTR_ISO 0x01 +#define USB_BM_ATTR_BULK 0x02 +#define USB_BM_ATTR_INTERRUPT 0x03 + + +/****************************************************************** + * Other stuff * + *****************************************************************/ + +#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)) + +#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} + +// 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{ + 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 + unsigned rx_cnt : 10; // received data counter +} ep_t; + +extern volatile uint8_t usbON; + +void USB_setup(); +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); + +// could be [re]defined in usb_dev.c +extern void usb_class_request(config_pack_t *packet, uint8_t *data, uint16_t datalen); +extern void usb_vendor_request(config_pack_t *packet, uint8_t *data, uint16_t datalen); +extern void set_configuration(); diff --git a/F3:F303/MLX90640/version.inc b/F3:F303/MLX90640/version.inc new file mode 100644 index 0000000..650158e --- /dev/null +++ b/F3:F303/MLX90640/version.inc @@ -0,0 +1,2 @@ +#define BUILD_NUMBER "13" +#define BUILD_DATE "2025-09-19"