fw: add basic driver for high-g accelerometer

3 files changed, 181 insertions, 5 deletions

diff --git a/prototype/fw/Makefile b/prototype/fw/Makefile
index 32ded36..f61e949 100644
--- a/prototype/fw/Makefile
+++ b/prototype/fw/Makefile
@@ -125,7 +125,7 @@ CXXFLAGS	+= -I.
 LDFLAGS			+= $(ARCH_FLAGS) $(SYSTEM_FLAGS)
 
 LIBS		+= -lgcc
-LDFLAGS		+= -Wl,--gc-sections
+#LDFLAGS		+= -Wl,--gc-sections
 
 LINKMEM_FLAGS ?= --trim-stubs=startup.o --trace-sections .isr_vector --highlight-subdirs $(BUILDDIR)
 
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;
diff --git a/prototype/fw/upstream/PyCortexMDebug b/prototype/fw/upstream/PyCortexMDebug
-Subproject 89c567b2847e1d3349b3bec64c3efa76f1ccf98
+Subproject b5ef1c8ed36af2e46452e4c3f81506c65b5c0ed