diff options
Diffstat (limited to 'prototype/fw/src')
| -rw-r--r-- | prototype/fw/src/main.c | 184 |
1 files changed, 180 insertions, 4 deletions
diff --git a/prototype/fw/src/main.c b/prototype/fw/src/main.c index 42c1ea7..cf0c9f4 100644 --- a/prototype/fw/src/main.c +++ b/prototype/fw/src/main.c @@ -38,13 +38,174 @@ void packetize(void *pkt, struct ll_pkt_trailer *trailer) { trailer->crc32 = pkt_crc(pkt, trailer); } +enum mems_regs { + MEMS_REG_CTRL0, /* 0 */ + MEMS_REG_CTRL1, /* 1 */ + MEMS_REG_CONFIG, /* 2 */ + MEMS_REG_STATUS0, /* 3 */ + MEMS_REG_STATUS1, /* 4 */ + MEMS_REG_STATUS2, /* 5 */ + MEMS_REG_CHIP_REVID, /* 6 */ + MEMS_REG_ACC_CHX_LOW, /* 7 */ + MEMS_REG_ACC_CHX_HIGH, /* 8 */ + MEMS_REG_ACC_CHY_LOW, /* 9 */ + MEMS_REG_ACC_CHY_HIGH, /* 10 */ + MEMS_REG_OSC_COUNTER, /* 11 */ + MEMS_REG_ID_SENSOR_TYPE, /* 12 */ + MEMS_REG_ID_VEH_MANUF, /* 13 */ + MEMS_REG_ID_SENSOR_MANUF, /* 14 */ + MEMS_REG_ID_LOT0, /* 15 */ + MEMS_REG_ID_LOT1, /* 16 */ + MEMS_REG_ID_LOT2, /* 17 */ + MEMS_REG_ID_LOT3, /* 18 */ + MEMS_REG_ID_WAFER, /* 19 */ + MEMS_REG_ID_COOR_X, /* 20 */ + MEMS_REG_ID_COOR_Y, /* 21 */ + MEMS_REG_RESET, /* 22 */ + MEMS_REG_OFF_CHX_HIGH, /* 23 */ + MEMS_REG_OFF_CHX_LOW, /* 24 */ + MEMS_REG_OFF_CHY_HIGH, /* 25 */ + MEMS_REG_OFF_CHY_LOW, /* 26 */ +}; + +uint8_t crc8_calc(uint8_t *data, size_t len) { + int acc = 0; + for (size_t i=0; i<len; i++) { + acc ^= data[i]; + for (size_t j=0; j<8; j++) { + acc <<= 1; + if (acc & 0x100) { + acc ^= 0x197; /* 0x100 | poly */ + } + } + } + if (acc & 0x100) + asm volatile ("bkpt"); + return acc; +} + +#define MEMS_OPCODE_Msk 0x3 +#define MEMS_OPCODE_Pos 30 +#define MEMS_ADDR_Msk 0x1f +#define MEMS_ADDR_Pos 21 +#define MEMS_DATA_Msk 0xff +#define MEMS_DATA_Pos 13 +#define MEMS_P_Pos 28 +#define MEMS_SEN_Pos 29 +#define MEMS_MEAS_Pos 12 +#define MEMS_MEAS_Msk 0x3fff + +bool parity_calc(uint8_t *data, size_t len) { + bool acc = 0; + for (size_t i=0; i<len; i++) { + uint8_t b = data[i]; + for (size_t j=0; j<8; j++) { + if (b&1) { + acc = !acc; + } + b >>= 1; + } + } + return acc; +} + +uint32_t mems_trx_word(uint32_t data) { + /* CAUTION: ST's SPI peripherals behave differently depending on DR register access size, yet the CMSIS headers + * expose it as an 32-bit uint only. In this case, we actually want a 32-bit access. + */ + uint16_t *dr = (uint16_t *)&SPI1->DR; + *dr = data>>16; + while (SPI1->SR & SPI_SR_BSY) + ; + uint32_t out = (*dr) << 16; + *dr = data&0xffff; + while (SPI1->SR & SPI_SR_BSY) + ; + out |= *dr; + return out; +} + +uint32_t mems_trx_cmd(uint32_t cmd) { + GPIOA->BRR = 1<<15; /* De-assert !CS */ + + uint8_t bytes[3] = {(cmd>>16)&0xff, (cmd>>8)&0xff, cmd&0xff}; + uint8_t crc = crc8_calc(bytes, 3); + int parity = !!parity_calc(bytes, 3); + + uint32_t out = mems_trx_word(cmd | (parity<<MEMS_P_Pos) | crc); + GPIOA->BSRR = 1<<15; /* Assert !CS */ + + return out; +} + +void mems_write_reg(int addr, int val) { + addr &= MEMS_ADDR_Msk; + val &= MEMS_DATA_Msk; + (void)mems_trx_cmd((1<<MEMS_OPCODE_Pos) | (addr<<MEMS_ADDR_Pos) | (val<<MEMS_DATA_Pos)); +} + +uint32_t mems_read_reg(int addr) { + addr &= MEMS_ADDR_Msk; + mems_trx_cmd((3<<MEMS_OPCODE_Pos) | (addr<<MEMS_ADDR_Pos)); + for (int i=0; i<2000; i++) + asm volatile ("nop"); + uint32_t rv = mems_trx_cmd(3<<MEMS_OPCODE_Pos); + return (rv >> MEMS_DATA_Pos) & MEMS_DATA_Msk; +} + +int16_t mems_read_meas(int ch) { + ch &= 3; + mems_trx_cmd((ch<<MEMS_OPCODE_Pos) | (1<<MEMS_SEN_Pos)); + for (int i=0; i<2000; i++) + asm volatile ("nop"); + uint32_t rv = mems_trx_cmd(3<<MEMS_OPCODE_Pos); + /* shift 14-bit data left to align the MSB with the int16_t's sign bit */ + int16_t data = (rv >> MEMS_MEAS_Pos) << 2; + /* Now do an arithmetic division to sign extend */ + return data / 4; +} + +void mems_spi_init(void) { + SPI1->CR1 = (6<<SPI_CR1_BR_Pos) | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI; + SPI1->CR2 = (15<<SPI_CR2_DS_Pos); + SPI1->CR1 |= SPI_CR1_SPE; +} + +void mems_init(void) { + mems_spi_init(); + + for (size_t i=0; i<10000; i++) { + asm volatile("nop"); + } + + /* Take accelerometer out of initialization phase */ + mems_write_reg(MEMS_REG_CTRL0, 0x01); +} + int main(void) { RCC->AHBENR |= RCC_AHBENR_GPIOAEN; - RCC->APB2ENR |= RCC_APB2ENR_USART1EN; + RCC->APB2ENR |= RCC_APB2ENR_USART1EN | RCC_APB2ENR_SPI1EN; - GPIOA->MODER |= (2 << GPIO_MODER_MODER9_Pos) | (2 << GPIO_MODER_MODER10_Pos); - GPIOA->AFR[1] = (7 << (9-8)*4) | (7 << (10-8)*4); +#define AFRL(pin, val) ((val) << ((pin)*4)) +#define AFRH(pin, val) ((val) << (((pin)-8)*4)) +#define AF(pin) (2<<(2*(pin))) +#define OUT(pin) (1<<(2*(pin))) +#define IN(pin) (0) +#define ANALOG(pin) (3<<(2*(pin))) +#define CLEAR(pin) (3<<(2*(pin))) + + /* GPIO pin config: + * A9: USART 1 TX -> LED + * A10: USART 1 RX -> debug + * A15: Accelerometer CS + * A5/6/7: SPI SCK/MISO/MOSI for Accelerometer + */ + GPIOA->MODER &= ~(CLEAR(15)); /* Clear JTAG TDI pin mode */ + GPIOA->MODER |= AF(9) | AF(10) | OUT(15) | AF(5) | AF(6) | AF(7); + GPIOA->AFR[0] = AFRL(5, 5) | AFRL(6, 5) | AFRL(7, 5); + GPIOA->AFR[1] = AFRH(9, 7) | AFRH(10, 7); + GPIOA->BSRR = 1<<15; /* De-assert accelerometer !CS */ SystemCoreClockUpdate(); int apb2_clock = SystemCoreClock / APB2_PRESC; @@ -56,6 +217,21 @@ int main(void) { USART1->CR2 |= USART_CR2_RXINV; //| USART_CR2_TXINV; USART1->CR1 |= USART_CR1_UE; + /* FIXME DEUBG */ + while (1) { + mems_init(); + for (int i=0; i<100000; i++) + asm volatile("nop"); + for (int i=0; i<300; i++) { + mems_read_meas(0); + //mems_read_reg(MEMS_REG_ID_SENSOR_TYPE); + for (int i=0; i<10000; i++) + asm volatile("nop"); + } + for (int i=0; i<100000; i++) + asm volatile("nop"); + } + int req_seq = 0; int res_seq = 0; struct req_pkt req_buf = { 0 }; @@ -118,7 +294,7 @@ int main(void) { } else { if (rc == sizeof(req_buf)) { crc32_t check_crc = pkt_crc(&req_buf, &req_buf.trailer); - if (check_crc != req_buf.trailer.crc32 || check_crc == 0 || check_crc == -1) { + if (check_crc != req_buf.trailer.crc32 || check_crc == 0 || (int)check_crc == -1) { rx_crc_error += 1; } else { req_seq = req_buf.req_seq; |