initial commit
This commit is contained in:
jhChun
541
project/ble_peripheral/ble_app_vivaMayo/cat_interface.c
Normal file
541
project/ble_peripheral/ble_app_vivaMayo/cat_interface.c
Normal file
@@ -0,0 +1,541 @@
|
||||
/*******************************************************************************
|
||||
* @file cat_interface.c
|
||||
* @brief
|
||||
******************************************************************************/
|
||||
|
||||
/* board driver */
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "nrf.h"
|
||||
#include "app_error.h"
|
||||
#include "boards.h"
|
||||
#include "nrfx_gpiote.h"
|
||||
#include "nrfx_twi.h"
|
||||
#include "nrf_crypto.h"
|
||||
#include "nrf_crypto_aes.h"
|
||||
|
||||
#include "nrf_drv_twi.h"
|
||||
#include "system_interface.h"
|
||||
#include "nrf_delay.h"
|
||||
#include "cat_interface.h"
|
||||
#include "debug_print.h"
|
||||
/* I2C number and slave address for INV device */
|
||||
#define ICM_I2C_ADDR 0x68
|
||||
#define INV_MAX_SERIAL_WRITE 16
|
||||
|
||||
|
||||
#define EEPROM_I2C_ADDRESS 0x50
|
||||
#define EEPROM_PAGE_SIZE 64
|
||||
#define EEPROM_INSTANCE 0
|
||||
|
||||
|
||||
|
||||
#define AES_BLOCK_SIZE 16
|
||||
|
||||
static uint8_t aes_key[16] = {
|
||||
0x60, 0x3d, 0xeb, 0x10, 0x15, 0xca, 0x71, 0xbe,
|
||||
0x2b, 0x73, 0xae, 0xf0, 0x85, 0x7d, 0x77, 0x81
|
||||
};
|
||||
|
||||
static uint8_t aes_iv[16] = { 0 }; // Fixed IV for simplicity
|
||||
|
||||
|
||||
|
||||
|
||||
const nrfx_twi_t m_eeprom = NRFX_TWI_INSTANCE(EEPROM_INSTANCE);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
ret_code_t encrypt_data(const uint8_t *input, size_t length, uint8_t *output, size_t *output_len) {
|
||||
nrf_crypto_aes_context_t aes_ctx;
|
||||
size_t padded_len = ((length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;
|
||||
|
||||
uint8_t buffer[padded_len];
|
||||
memcpy(buffer, input, length);
|
||||
memset(&buffer[length], 0, padded_len - length); // Zero padding
|
||||
|
||||
ret_code_t err = nrf_crypto_aes_init(&aes_ctx, &g_nrf_crypto_aes_cbc_128_info, NRF_CRYPTO_ENCRYPT);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
err = nrf_crypto_aes_key_set(&aes_ctx, aes_key);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
err = nrf_crypto_aes_iv_set(&aes_ctx, aes_iv);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
*output_len = padded_len;
|
||||
err = nrf_crypto_aes_finalize(&aes_ctx, buffer, padded_len, output, output_len);
|
||||
return err;
|
||||
}
|
||||
|
||||
ret_code_t decrypt_data(const uint8_t *input, size_t length, uint8_t *output) {
|
||||
nrf_crypto_aes_context_t aes_ctx;
|
||||
uint8_t input_copy[length];
|
||||
memcpy(input_copy, input, length);
|
||||
|
||||
ret_code_t err = nrf_crypto_aes_init(&aes_ctx, &g_nrf_crypto_aes_cbc_128_info, NRF_CRYPTO_DECRYPT);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
err = nrf_crypto_aes_key_set(&aes_ctx, aes_key);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
err = nrf_crypto_aes_iv_set(&aes_ctx, aes_iv);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
size_t output_len = length;
|
||||
err = nrf_crypto_aes_finalize(&aes_ctx, input_copy, length, output, &output_len);
|
||||
return err;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_write_encrypted(uint16_t mem_address, const uint8_t *plaintext, size_t length) {
|
||||
uint8_t encrypted_buf[256]; // Adjust if needed
|
||||
size_t encrypted_len = 0;
|
||||
|
||||
ret_code_t err = encrypt_data(plaintext, length, encrypted_buf, &encrypted_len);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
return eeprom_write_bytes(mem_address, encrypted_buf, encrypted_len);
|
||||
}
|
||||
|
||||
|
||||
ret_code_t eeprom_read_decrypted(uint16_t mem_address, uint8_t *plaintext, size_t original_length) {
|
||||
size_t encrypted_len = ((original_length + AES_BLOCK_SIZE - 1) / AES_BLOCK_SIZE) * AES_BLOCK_SIZE;
|
||||
uint8_t encrypted_buf[256]; // Adjust as needed
|
||||
|
||||
ret_code_t err = eeprom_read_bytes(mem_address, encrypted_buf, encrypted_len);
|
||||
VERIFY_SUCCESS(err);
|
||||
|
||||
return decrypt_data(encrypted_buf, encrypted_len, plaintext);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void eeprom_uninitialize(void){
|
||||
nrfx_twi_disable(&m_eeprom);
|
||||
nrfx_twi_uninit(&m_eeprom);
|
||||
}
|
||||
|
||||
void eeprom_initialize(void){
|
||||
ret_code_t err_code;
|
||||
|
||||
const nrfx_twi_config_t eeprom_config = {
|
||||
.scl = ICM42670_I2C_SCL_PIN,
|
||||
.sda = ICM42670_I2C_SDA_PIN,
|
||||
.frequency = NRF_TWI_FREQ_400K,
|
||||
.interrupt_priority = APP_IRQ_PRIORITY_HIGH,
|
||||
};
|
||||
|
||||
err_code = nrfx_twi_init(&m_eeprom, &eeprom_config, NULL, NULL);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
nrfx_twi_enable(&m_eeprom);
|
||||
}
|
||||
|
||||
ret_code_t eeprom_initialize_safe(void){
|
||||
ret_code_t err_code;
|
||||
|
||||
const nrfx_twi_config_t eeprom_config = {
|
||||
.scl = ICM42670_I2C_SCL_PIN,
|
||||
.sda = ICM42670_I2C_SDA_PIN,
|
||||
.frequency = NRF_TWI_FREQ_400K,
|
||||
.interrupt_priority = APP_IRQ_PRIORITY_HIGH,
|
||||
};
|
||||
|
||||
err_code = nrfx_twi_init(&m_eeprom, &eeprom_config, NULL, NULL);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
return err_code;
|
||||
}
|
||||
|
||||
nrfx_twi_enable(&m_eeprom);
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_write_page(uint16_t mem_address, const uint8_t *data)
|
||||
{
|
||||
|
||||
uint8_t buffer[2 + EEPROM_PAGE_SIZE]; // 2 bytes for address + 64 bytes data
|
||||
ret_code_t ret;
|
||||
|
||||
buffer[0] = (uint8_t)(mem_address >> 8); // MSB
|
||||
buffer[1] = (uint8_t)(mem_address & 0xFF); // LSB
|
||||
memcpy(&buffer[2], data, EEPROM_PAGE_SIZE);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 2 + EEPROM_PAGE_SIZE, false);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM write failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Wait for internal EEPROM write cycle (typically ~5ms)
|
||||
for (int i = 0; i < 100; i++) {
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
|
||||
if (ret == NRF_SUCCESS)
|
||||
break;
|
||||
nrf_delay_us(100); // Wait 100us before retry
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
ret_code_t eeprom_write_uint16_array(uint16_t start_address, const uint16_t *data, size_t count)
|
||||
{
|
||||
|
||||
if (count != 48) {
|
||||
DBG_PRINTF("Error: This function is only for writing exactly 48 uint16_t values.\n");
|
||||
return NRF_ERROR_INVALID_PARAM;
|
||||
}
|
||||
if (start_address < 128) {
|
||||
DBG_PRINTF("Error: This function is only for after 192....\n");
|
||||
return NRF_ERROR_INVALID_PARAM;
|
||||
}
|
||||
|
||||
uint8_t buffer[EEPROM_PAGE_SIZE];
|
||||
ret_code_t ret;
|
||||
|
||||
// Write first 32 uint16_t values (64 bytes)
|
||||
for (size_t i = 0; i < 32; i++) {
|
||||
buffer[i * 2] = (uint8_t)(data[i] >> 8); // MSB
|
||||
buffer[i * 2 + 1] = (uint8_t)(data[i] & 0xFF); // LSB
|
||||
// buffer[i * 2 +1] = (uint8_t)(data[i] >> 8); // MSB
|
||||
// buffer[i * 2 ] = (uint8_t)(data[i] & 0xFF); // LSB
|
||||
}
|
||||
|
||||
ret = eeprom_write_page(start_address, buffer);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Write remaining 16 uint16_t values (32 bytes)
|
||||
for (size_t i = 0; i < 16; i++) {
|
||||
buffer[i * 2] = (uint8_t)(data[i + 32] >> 8);
|
||||
buffer[i * 2 + 1] = (uint8_t)(data[i + 32] & 0xFF);
|
||||
}
|
||||
|
||||
ret = eeprom_write_page(start_address + EEPROM_PAGE_SIZE, buffer); // next page
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
|
||||
ret_code_t eeprom_read_uint16_array(uint16_t start_address, uint16_t *data, size_t count)
|
||||
{
|
||||
|
||||
|
||||
if (count != 48) {
|
||||
DBG_PRINTF("Error: This function is only for reading exactly 48 uint16_t values.\n");
|
||||
return NRF_ERROR_INVALID_PARAM;
|
||||
}
|
||||
|
||||
uint8_t buffer[EEPROM_PAGE_SIZE];
|
||||
ret_code_t ret;
|
||||
|
||||
// Read first 64 bytes (32 uint16_t)
|
||||
ret = eeprom_read_bytes(start_address, buffer, EEPROM_PAGE_SIZE);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < 32; i++) {
|
||||
data[i] = ((uint16_t)buffer[i * 2] << 8) | buffer[i * 2 + 1];
|
||||
}
|
||||
|
||||
// Read next 32 bytes (16 uint16_t)
|
||||
ret = eeprom_read_bytes(start_address + EEPROM_PAGE_SIZE, buffer, 32);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < 16; i++) {
|
||||
data[i + 32] = ((uint16_t)buffer[i * 2] << 8) | buffer[i * 2 + 1];
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_read_page(uint16_t mem_address, uint8_t *data)
|
||||
{
|
||||
|
||||
uint8_t addr_buf[2];
|
||||
ret_code_t ret;
|
||||
|
||||
addr_buf[0] = (uint8_t)(mem_address >> 8);
|
||||
addr_buf[1] = (uint8_t)(mem_address & 0xFF);
|
||||
|
||||
// Send memory address first
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM set read address failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read the page
|
||||
ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, EEPROM_PAGE_SIZE);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
DBG_PRINTF("EEPROM read \n");
|
||||
for (int i = 0; i < EEPROM_PAGE_SIZE; i++) {
|
||||
|
||||
DBG_PRINTF("%02X\n", data[i]);
|
||||
}
|
||||
DBG_PRINTF("\r\n");
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
ret_code_t eeprom_write_byte(uint16_t mem_address, uint8_t data)
|
||||
{
|
||||
|
||||
uint8_t buffer[3]; // 2 bytes for address + 1 byte data
|
||||
ret_code_t ret;
|
||||
|
||||
buffer[0] = (uint8_t)(mem_address >> 8); // MSB of address
|
||||
buffer[1] = (uint8_t)(mem_address & 0xFF); // LSB of address
|
||||
buffer[2] = data;
|
||||
DBG_PRINTF("EEPROM write byte %02X,%02X,%02X\n", buffer[0], buffer[1], buffer[2]);
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 3, false);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM write byte failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Wait for internal EEPROM write cycle (typically ~5ms)
|
||||
for (int i = 0; i < 100; i++) {
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
|
||||
if (ret == NRF_SUCCESS)
|
||||
break;
|
||||
nrf_delay_us(100); // Wait 100us before retry
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_read_byte(uint16_t mem_address, uint8_t *data)
|
||||
{
|
||||
|
||||
uint8_t addr_buf[2];
|
||||
ret_code_t ret;
|
||||
|
||||
addr_buf[0] = (uint8_t)(mem_address >> 8); // MSB of address
|
||||
addr_buf[1] = (uint8_t)(mem_address & 0xFF); // LSB of address
|
||||
|
||||
// Send memory address
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM set address failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read 1 byte
|
||||
ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, 1);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read byte failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
ret_code_t eeprom_write_bytes(uint16_t mem_address, const uint8_t *data, size_t length)
|
||||
{
|
||||
|
||||
ret_code_t ret;
|
||||
size_t bytes_written = 0;
|
||||
|
||||
while (bytes_written < length) {
|
||||
size_t page_offset = mem_address % EEPROM_PAGE_SIZE;
|
||||
size_t bytes_to_write = EEPROM_PAGE_SIZE - page_offset;
|
||||
if (bytes_to_write > (length - bytes_written)) {
|
||||
bytes_to_write = length - bytes_written;
|
||||
}
|
||||
|
||||
uint8_t buffer[2 + EEPROM_PAGE_SIZE]; // 2-byte addr + up to 64 data bytes
|
||||
buffer[0] = (uint8_t)(mem_address >> 8);
|
||||
buffer[1] = (uint8_t)(mem_address & 0xFF);
|
||||
memcpy(&buffer[2], &data[bytes_written], bytes_to_write);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, bytes_to_write + 2, false);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM write error at addr 0x%04X\n", mem_address);
|
||||
return ret;
|
||||
}
|
||||
// Wait for internal EEPROM write cycle (typically ~5ms)
|
||||
for (int i = 0; i < 100; i++) {
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
|
||||
if (ret == NRF_SUCCESS)
|
||||
break;
|
||||
nrf_delay_us(100); // Wait 100us before retry
|
||||
}
|
||||
|
||||
mem_address += bytes_to_write;
|
||||
bytes_written += bytes_to_write;
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_read_bytes(uint16_t mem_address, uint8_t *data, size_t length)
|
||||
{
|
||||
|
||||
ret_code_t ret;
|
||||
uint8_t addr_buf[2];
|
||||
|
||||
addr_buf[0] = (uint8_t)(mem_address >> 8);
|
||||
addr_buf[1] = (uint8_t)(mem_address & 0xFF);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true); // send addr, no stop
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read set address failed\n");
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data, length);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read failed\n");
|
||||
return ret;
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_write_word(uint16_t mem_address, uint16_t data)
|
||||
{
|
||||
|
||||
uint8_t buffer[4]; // 2 bytes for address + 2 bytes for data
|
||||
ret_code_t ret;
|
||||
|
||||
buffer[0] = (uint8_t)(mem_address >> 8); // MSB of address
|
||||
buffer[1] = (uint8_t)(mem_address & 0xFF); // LSB of address
|
||||
buffer[2] = (uint8_t)(data & 0xFF); // LSB of data
|
||||
buffer[3] = (uint8_t)(data >> 8); // MSB of data
|
||||
|
||||
DBG_PRINTF("EEPROM write word %02X,%02X,%02X,%02X\n", buffer[0], buffer[1], buffer[2], buffer[3]);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 4, false);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM write word failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Wait for internal write cycle (~5ms typical)
|
||||
for (int i = 0; i < 100; i++) {
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
|
||||
if (ret == NRF_SUCCESS)
|
||||
break;
|
||||
nrf_delay_us(100); // 100us delay between polling
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
ret_code_t eeprom_read_word(uint16_t mem_address, uint16_t *data)
|
||||
{
|
||||
|
||||
uint8_t addr_buf[2];
|
||||
uint8_t read_buf[2];
|
||||
ret_code_t ret;
|
||||
|
||||
addr_buf[0] = (uint8_t)(mem_address >> 8);
|
||||
addr_buf[1] = (uint8_t)(mem_address & 0xFF);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM set address for word read failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, read_buf, 2);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read word failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
*data = ((uint16_t)read_buf[1] << 8) | read_buf[0]; // Little-endian
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
ret_code_t eeprom_write_uint32(uint16_t mem_address, uint32_t data)
|
||||
{
|
||||
|
||||
uint8_t buffer[6]; // 2 bytes address + 4 bytes data
|
||||
ret_code_t ret;
|
||||
|
||||
buffer[0] = (uint8_t)(mem_address >> 8); // MSB of address
|
||||
buffer[1] = (uint8_t)(mem_address & 0xFF); // LSB of address
|
||||
buffer[2] = (uint8_t)(data >> 24);
|
||||
buffer[3] = (uint8_t)(data >> 16);
|
||||
buffer[4] = (uint8_t)(data >> 8);
|
||||
buffer[5] = (uint8_t)(data);
|
||||
|
||||
DBG_PRINTF("EEPROM write uint32: %02X %02X %02X %02X %02X %02X\n",
|
||||
buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]);
|
||||
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, buffer, 6, false);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM write uint32 failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Wait for internal EEPROM write cycle
|
||||
for (int i = 0; i < 100; i++) {
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, NULL, 0, false);
|
||||
if (ret == NRF_SUCCESS)
|
||||
break;
|
||||
nrf_delay_us(100);
|
||||
}
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
ret_code_t eeprom_read_uint32(uint16_t mem_address, uint32_t *data)
|
||||
{
|
||||
uint8_t addr_buf[2];
|
||||
uint8_t data_buf[4];
|
||||
ret_code_t ret;
|
||||
|
||||
|
||||
addr_buf[0] = (uint8_t)(mem_address >> 8);
|
||||
addr_buf[1] = (uint8_t)(mem_address & 0xFF);
|
||||
|
||||
DBG_PRINTF("EEPROM address:%02X,%02X \n",addr_buf[0],addr_buf[1]);
|
||||
// Send memory address to read from
|
||||
ret = icm42670_twi_tx(EEPROM_I2C_ADDRESS, addr_buf, 2, true);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read (addr phase) failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Read 4 bytes of data
|
||||
ret = icm42670_twi_rx(EEPROM_I2C_ADDRESS, data_buf, 4);
|
||||
if (ret != NRF_SUCCESS) {
|
||||
DBG_PRINTF("EEPROM read (data phase) failed (code: %d)\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
*data = ((uint32_t)data_buf[0] << 24) |
|
||||
((uint32_t)data_buf[1] << 16) |
|
||||
((uint32_t)data_buf[2] << 8) |
|
||||
((uint32_t)data_buf[3]);
|
||||
|
||||
DBG_PRINTF("EEPROM read uint32: %02X %02X %02X %02X -> %08X\n",
|
||||
data_buf[0], data_buf[1], data_buf[2], data_buf[3], *data);
|
||||
|
||||
return NRF_SUCCESS;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user