mikkeli/pi-dashboard
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
19db1256196c51447e9daddf6abb1817de18d86b
pi-dashboard/components/ExternLib/SensorLib/src/bosch/bhy2_hif.c

1764 lines
54 KiB
C
Raw Normal View History

initial commit
2026-02-15 02:48:59 +09:00
/**
* Copyright (c) 2023 Bosch Sensortec GmbH. All rights reserved.
*
* BSD-3-Clause
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* @file bhy2_hif.c
* @date 2023-03-24
* @version v1.6.0
*
*/
#include "bhy2_hif.h"
/*! Mask definitions for SPI read/write address */
#define BHY2_SPI_RD_MASK UINT8_C(0x80)
#define BHY2_SPI_WR_MASK UINT8_C(0x7F)
union bhy2_hif_float_u32
{
bhy2_float as_float;
uint32_t reg;
};
static void bhy2_hif_exec_cmd_generic_support(const uint16_t *cmd,
uint8_t *cmd_buf,
uint32_t *temp_length,
uint32_t *total_length,
const uint32_t *len,
const uint32_t *pre_len,
const uint32_t *cmd_len)
{
*total_length = *pre_len + *len;
if (*cmd_len)
{
*temp_length = *pre_len + *cmd_len;
}
else
{
*temp_length = *total_length;
}
/* Align 4 bytes */
if (*temp_length % 4)
{
*temp_length = BHY2_ROUND_WORD_HIGHER(*temp_length);
}
cmd_buf[0] = (uint8_t)(*cmd & 0xFF);
cmd_buf[1] = (uint8_t)((*cmd >> 8) & 0xFF);
/* Length in word */
if (*cmd == BHY2_CMD_UPLOAD_TO_PROGRAM_RAM)
{
cmd_buf[2] = (*temp_length / 4) & 0xFF;
cmd_buf[3] = ((*temp_length / 4) >> 8) & 0xFF;
}
/* Length in byte */
else
{
cmd_buf[2] = *temp_length & 0xFF;
cmd_buf[3] = (*temp_length >> 8) & 0xFF;
}
}
/*
* If a command need several frames to upload to sensor,
* this command only be used in the first frame.
*
* |------------------------------------------------|---------------|
* |<-- frame one -->|<--frame two-->|
* |------------------------------------------------|---------------|
* | |<--pre_length-->|<--length-->| |
* |------------------|----------------|------------|---------------|
* | cmd | cmd_length | pre_payload | data 1 | data 2 |
* |-----------------------------------|------------|---------------|
* | payload |
* |------------|---------------|
* |<-- cmd_length -->|
* |----------------------------|
*/
static int8_t bhy2_hif_exec_cmd_generic(uint16_t cmd,
const uint8_t *payload,
uint32_t length,
const uint8_t *pre_payload,
uint32_t pre_length,
uint32_t cmd_length,
struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint32_t remain, trans_len, copy_len, pos, total_len, temp_total_len, loop_remain_len, max_len;
uint8_t command_buf[BHY2_COMMAND_PACKET_LEN];
if ((hif != NULL) && !((length != 0) && (payload == NULL)) && !((pre_length != 0) && (pre_payload == NULL)))
{
if (hif->read_write_len != 0)
{
bhy2_hif_exec_cmd_generic_support(&cmd,
command_buf,
&temp_total_len,
&total_len,
&length,
&pre_length,
&cmd_length);
pos = BHY2_COMMAND_HEADER_LEN;
remain = total_len;
loop_remain_len = remain + pos;
max_len = BHY2_COMMAND_PACKET_LEN - BHY2_COMMAND_HEADER_LEN;
if (hif->read_write_len < max_len)
{
max_len = hif->read_write_len;
}
while ((loop_remain_len > 0) && (rslt == BHY2_OK))
{
if ((remain + pos) > max_len)
{
trans_len = max_len;
copy_len = max_len - pos;
}
else
{
trans_len = remain + pos;
copy_len = remain;
/* Align to 4 bytes */
if (trans_len % 4)
{
trans_len = BHY2_ROUND_WORD_HIGHER(trans_len);
}
if (trans_len > (BHY2_COMMAND_PACKET_LEN - BHY2_COMMAND_HEADER_LEN))
{
trans_len = BHY2_COMMAND_PACKET_LEN - BHY2_COMMAND_HEADER_LEN;
}
}
if (copy_len > 0)
{
if (remain >= (length + copy_len))
{
memcpy(&command_buf[pos], &pre_payload[total_len - remain], copy_len);
}
else if (remain > length)
{
memcpy(&command_buf[pos], &pre_payload[total_len - remain], remain - length);
memcpy(&command_buf[pos + remain - length], payload, copy_len - (remain - length));
}
else
{
memcpy(&command_buf[pos], &payload[length - remain], copy_len);
}
}
if ((trans_len - (pos + copy_len)) > 0)
{
memset(&command_buf[pos + copy_len], 0, BHY2_COMMAND_PACKET_LEN - (pos + copy_len));
}
rslt = bhy2_hif_set_regs(BHY2_REG_CHAN_CMD, command_buf, trans_len, hif);
if (rslt != BHY2_OK)
{
break;
}
pos = 0;
remain -= copy_len;
loop_remain_len = remain;
}
}
else
{
rslt = BHY2_E_INVALID_PARAM;
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_wait_status_ready(struct bhy2_hif_dev *hif)
{
uint16_t retry;
uint8_t int_status;
int8_t rslt;
/* Wait status ready */
for (retry = 0; retry < BHY2_QUERY_PARAM_STATUS_READY_MAX_RETRY; ++retry)
{
rslt = bhy2_hif_get_interrupt_status(&int_status, hif);
if (rslt == BHY2_OK)
{
if (int_status & BHY2_IST_MASK_STATUS)
{
rslt = BHY2_OK;
break;
}
/* 10ms */
rslt = bhy2_hif_delay_us(10000, hif);
if (rslt != BHY2_OK)
{
return rslt;
}
}
else
{
return rslt; /*break; */
}
}
return rslt;
}
static int8_t bhy2_hif_check_boot_status_ram(struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint16_t i;
uint8_t boot_status;
/* total 5s */
for (i = 0; i < BHY2_BST_CHECK_RETRY; ++i)
{
/* 50ms */
rslt = bhy2_hif_delay_us(50000, hif);
if (rslt < 0)
{
return rslt;
}
rslt = bhy2_hif_get_regs(BHY2_REG_BOOT_STATUS, &boot_status, sizeof(boot_status), hif);
if (rslt < 0)
{
return rslt;
}
if ((boot_status & BHY2_BST_HOST_INTERFACE_READY) && (boot_status & BHY2_BST_HOST_FW_VERIFY_DONE) &&
(!(boot_status & BHY2_BST_HOST_FW_VERIFY_ERROR)))
{
break;
}
}
if (i == BHY2_BST_CHECK_RETRY)
{
return BHY2_E_TIMEOUT;
}
return BHY2_OK;
}
static int8_t bhy2_hif_check_boot_status_flash(struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint16_t i;
uint8_t boot_status;
/* total 5s */
for (i = 0; i < BHY2_BST_CHECK_RETRY; ++i)
{
/* 50ms */
rslt = bhy2_hif_delay_us(50000, hif);
if (rslt < 0)
{
return rslt;
}
rslt = bhy2_hif_get_regs(BHY2_REG_BOOT_STATUS, &boot_status, sizeof(boot_status), hif);
if (rslt < 0)
{
return rslt;
}
if ((boot_status & BHY2_BST_HOST_INTERFACE_READY) && (boot_status & BHY2_BST_FLASH_VERIFY_DONE) &&
(!(boot_status & BHY2_BST_FLASH_VERIFY_ERROR)))
{
break;
}
}
if (i == BHY2_BST_CHECK_RETRY)
{
return BHY2_E_TIMEOUT;
}
return BHY2_OK;
}
static int8_t bhy2_hif_get_fifo(uint8_t reg,
uint8_t *fifo,
uint32_t fifo_len,
uint32_t *bytes_read,
uint32_t *bytes_remain,
struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint8_t n_bytes[2];
uint32_t read_len;
uint32_t read_write_len;
uint32_t offset;
if ((hif != NULL) && (fifo != NULL) && (bytes_read != NULL) && (bytes_remain != NULL))
{
read_write_len = hif->read_write_len;
if (*bytes_remain == 0)
{
rslt = bhy2_hif_get_regs(reg, n_bytes, 2, hif); /* Read the the available size */
*bytes_remain = BHY2_LE2U16(n_bytes);
}
if ((*bytes_remain != 0) && (rslt == BHY2_OK))
{
if (fifo_len < *bytes_remain)
{
*bytes_read = fifo_len;
}
else
{
*bytes_read = *bytes_remain;
}
read_len = *bytes_read;
offset = 0;
while (read_len > read_write_len)
{
rslt = bhy2_hif_get_regs(reg, &fifo[offset], read_write_len, hif);
if (rslt != BHY2_OK)
{
break;
}
read_len -= read_write_len;
offset += read_write_len;
}
if (read_len != 0)
{
rslt = bhy2_hif_get_regs(reg, &fifo[offset], read_len, hif);
}
*bytes_remain -= *bytes_read;
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_init(enum bhy2_intf intf,
bhy2_read_fptr_t read,
bhy2_write_fptr_t write,
bhy2_delay_us_fptr_t delay_us,
uint32_t read_write_len,
void *intf_ptr,
struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
if ((hif != NULL) && (read != NULL) && (write != NULL) && (delay_us != NULL))
{
hif->read = read;
hif->write = write;
hif->delay_us = delay_us;
hif->intf_ptr = intf_ptr;
hif->intf = intf;
if (read_write_len % 4)
{
hif->read_write_len = BHY2_ROUND_WORD_LOWER(read_write_len);
}
else
{
hif->read_write_len = read_write_len;
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
if ((hif != NULL) && (hif->read != NULL) && (reg_data != NULL))
{
if (hif->intf == BHY2_SPI_INTERFACE)
{
reg_addr |= BHY2_SPI_RD_MASK;
}
hif->intf_rslt = hif->read(reg_addr, reg_data, length, hif->intf_ptr);
if (hif->intf_rslt != BHY2_INTF_RET_SUCCESS)
{
rslt = BHY2_E_IO;
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
if ((hif != NULL) && (hif->write != NULL) && (reg_data != NULL))
{
if (hif->intf == BHY2_SPI_INTERFACE)
{
reg_addr &= BHY2_SPI_WR_MASK;
}
hif->intf_rslt = hif->write(reg_addr, reg_data, length, hif->intf_ptr);
if (hif->intf_rslt != BHY2_INTF_RET_SUCCESS)
{
rslt = BHY2_E_IO;
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_delay_us(uint32_t period_us, const struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
if ((hif != NULL) && (hif->delay_us != NULL))
{
hif->delay_us(period_us, hif->intf_ptr);
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_exec_cmd(uint16_t cmd, const uint8_t *payload, uint32_t length, struct bhy2_hif_dev *hif)
{
return bhy2_hif_exec_cmd_generic(cmd, payload, length, NULL, 0, 0, hif);
}
int8_t bhy2_hif_get_parameter(uint16_t param,
uint8_t *payload,
uint32_t payload_len,
uint32_t *actual_len,
struct bhy2_hif_dev *hif)
{
uint16_t code = 0;
uint8_t prev_hif_ctrl, hif_ctrl;
int8_t rslt = BHY2_OK;
if ((hif != NULL) && (payload != NULL) && (actual_len != NULL))
{
*actual_len = 0;
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &hif_ctrl, 1, hif);
if (rslt == BHY2_OK)
{
prev_hif_ctrl = hif_ctrl;
hif_ctrl &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL)); /* Change the Status FIFO to synchronous mode
* */
if (hif_ctrl != prev_hif_ctrl)
{
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &hif_ctrl, 1, hif);
}
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_exec_cmd(param | BHY2_PARAM_READ_MASK, NULL, 0, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(&code, payload, payload_len, actual_len, hif);
if (rslt == BHY2_OK)
{
if (hif_ctrl != prev_hif_ctrl)
{
hif_ctrl = prev_hif_ctrl;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &hif_ctrl, 1, hif);
}
if (rslt == BHY2_OK)
{
if (code != param)
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_set_parameter(uint16_t param, const uint8_t *payload, uint32_t length, struct bhy2_hif_dev *hif)
{
return bhy2_hif_exec_cmd(param, payload, length, hif);
}
int8_t bhy2_hif_get_product_id(uint8_t *product_id, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_PRODUCT_ID, product_id, 1, hif);
}
int8_t bhy2_hif_get_chip_id(uint8_t *chip_id, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_CHIP_ID, chip_id, 1, hif);
}
int8_t bhy2_hif_get_rom_version(uint16_t *rom_version, struct bhy2_hif_dev *hif)
{
uint8_t buffer[2];
int8_t rslt;
if (rom_version != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_ROM_VERSION_0, buffer, 2, hif);
*rom_version = BHY2_LE2U16(buffer);
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_kernel_version(uint16_t *kernel_version, struct bhy2_hif_dev *hif)
{
uint8_t buffer[2];
int8_t rslt;
if (kernel_version != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_KERNEL_VERSION_0, buffer, 2, hif);
*kernel_version = BHY2_LE2U16(buffer);
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_user_version(uint16_t *user_version, struct bhy2_hif_dev *hif)
{
uint8_t buffer[2];
int8_t rslt;
if (user_version != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_USER_VERSION_0, buffer, 2, hif);
*user_version = BHY2_LE2U16(buffer);
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_boot_status(uint8_t *boot_status, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_BOOT_STATUS, boot_status, 1, hif);
}
int8_t bhy2_hif_get_host_status(uint8_t *host_status, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_HOST_STATUS, host_status, 1, hif);
}
int8_t bhy2_hif_get_feature_status(uint8_t *feat_status, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_FEATURE_STATUS, feat_status, 1, hif);
}
int8_t bhy2_hif_get_interrupt_status(uint8_t *int_status, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_INT_STATUS, int_status, 1, hif);
}
int8_t bhy2_hif_get_fw_error(uint8_t *fw_error, struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_regs(BHY2_REG_ERROR_VALUE, fw_error, 1, hif);
}
int8_t bhy2_hif_reset(struct bhy2_hif_dev *hif)
{
uint8_t reset_req = BHY2_REQUEST_RESET;
uint8_t boot_status = 0;
int8_t rslt;
/* Timeout at 150ms (15 * 10000 microseconds) */
uint16_t count = 15;
rslt = bhy2_hif_set_regs(BHY2_REG_RESET_REQ, &reset_req, 1, hif);
if (rslt == BHY2_OK)
{
while (count--)
{
rslt = bhy2_hif_delay_us(10000, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_regs(BHY2_REG_BOOT_STATUS, &boot_status, 1, hif);
if (rslt == BHY2_OK)
{
if (boot_status & BHY2_BST_HOST_INTERFACE_READY)
{
break;
}
}
}
}
if (!(boot_status & BHY2_BST_HOST_INTERFACE_READY))
{
rslt = BHY2_E_TIMEOUT;
}
}
return rslt;
}
int8_t bhy2_hif_upload_firmware_to_ram(const uint8_t *firmware, uint32_t length, struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint16_t magic;
if ((hif != NULL) && (firmware != NULL))
{
magic = BHY2_LE2U16(firmware);
if (magic != BHY2_FW_MAGIC)
{
rslt = BHY2_E_MAGIC;
}
else
{
rslt = bhy2_hif_exec_cmd(BHY2_CMD_UPLOAD_TO_PROGRAM_RAM, firmware, length, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_check_boot_status_ram(hif);
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_upload_firmware_to_ram_partly(const uint8_t *firmware,
uint32_t total_size,
uint32_t cur_pos,
uint32_t packet_len,
struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint16_t magic;
if ((hif != NULL) && (firmware != NULL))
{
if (cur_pos == 0)
{
magic = BHY2_LE2U16(firmware);
if (magic != BHY2_FW_MAGIC)
{
rslt = BHY2_E_MAGIC;
}
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_exec_cmd_generic(BHY2_CMD_UPLOAD_TO_PROGRAM_RAM,
firmware,
packet_len,
NULL,
0,
total_size,
hif);
}
}
else
{
rslt = bhy2_hif_set_regs(BHY2_REG_CHAN_CMD, firmware, packet_len, hif);
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_boot_program_ram(struct bhy2_hif_dev *hif)
{
int8_t rslt;
rslt = bhy2_hif_exec_cmd(BHY2_CMD_BOOT_PROGRAM_RAM, NULL, 0, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_check_boot_status_ram(hif);
}
return rslt;
}
int8_t bhy2_hif_get_wakeup_fifo(uint8_t *fifo,
uint32_t fifo_len,
uint32_t *bytes_read,
uint32_t *bytes_remain,
struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_fifo(BHY2_REG_CHAN_FIFO_W, fifo, fifo_len, bytes_read, bytes_remain, hif);
}
int8_t bhy2_hif_get_nonwakeup_fifo(uint8_t *fifo,
uint32_t fifo_len,
uint32_t *bytes_read,
uint32_t *bytes_remain,
struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_fifo(BHY2_REG_CHAN_FIFO_NW, fifo, fifo_len, bytes_read, bytes_remain, hif);
}
int8_t bhy2_hif_get_status_fifo(uint16_t *status_code,
uint8_t *fifo,
uint32_t fifo_len,
uint32_t *bytes_remain,
struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint8_t tmp_fifo[4];
rslt = bhy2_hif_get_regs(BHY2_REG_CHAN_STATUS, tmp_fifo, 4, hif);
if (rslt == BHY2_OK)
{
*status_code = BHY2_LE2U16(&tmp_fifo[0]);
*bytes_remain = BHY2_LE2U16(&tmp_fifo[2]);
if (*bytes_remain != 0)
{
if (fifo_len < *bytes_remain)
{
rslt = BHY2_E_BUFFER;
}
else
{
rslt = bhy2_hif_get_regs(BHY2_REG_CHAN_STATUS, fifo, *bytes_remain, hif);
}
}
}
return rslt;
}
int8_t bhy2_hif_get_status_fifo_async(uint8_t *fifo,
uint32_t fifo_len,
uint32_t *bytes_read,
uint32_t *bytes_remain,
struct bhy2_hif_dev *hif)
{
return bhy2_hif_get_fifo(BHY2_REG_CHAN_STATUS, fifo, fifo_len, bytes_read, bytes_remain, hif);
}
int8_t bhy2_hif_exec_sensor_conf_cmd(uint8_t sensor_id,
bhy2_float sample_rate,
uint32_t latency,
struct bhy2_hif_dev *hif)
{
uint8_t tmp_buf[8];
union bhy2_hif_float_u32 sample_rate_u;
sample_rate_u.as_float = sample_rate;
/* Sample rate is 32bits, latency is 24bits */
tmp_buf[0] = sensor_id;
tmp_buf[1] = (uint8_t)(sample_rate_u.reg & 0xFF);
tmp_buf[2] = (uint8_t)((sample_rate_u.reg >> 8) & 0xFF);
tmp_buf[3] = (uint8_t)((sample_rate_u.reg >> 16) & 0xFF);
tmp_buf[4] = (uint8_t)((sample_rate_u.reg >> 24) & 0xFF);
tmp_buf[5] = (uint8_t)(latency & 0xFF);
tmp_buf[6] = (uint8_t)((latency >> 8) & 0xFF);
tmp_buf[7] = (uint8_t)((latency >> 16) & 0xFF);
return bhy2_hif_exec_cmd(BHY2_CMD_CONFIG_SENSOR, tmp_buf, 8, hif);
}
int8_t bhy2_hif_set_fifo_flush(uint8_t sensor_id, struct bhy2_hif_dev *hif)
{
uint8_t tmp_buf[4] = {sensor_id, 0, 0, 0};
return bhy2_hif_exec_cmd(BHY2_CMD_FIFO_FLUSH, tmp_buf, 4, hif);
}
int8_t bhy2_hif_exec_soft_passthrough(const uint8_t *payload,
uint32_t payload_len,
uint8_t *reg_data,
uint32_t length,
uint32_t *actual_len,
struct bhy2_hif_dev *hif)
{
uint16_t code = 0;
uint8_t old_status, tmp_buf;
int8_t rslt;
/* Enter synchronous mode */
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
old_status = tmp_buf;
tmp_buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
/* Execute the Software passthrough */
rslt = bhy2_hif_exec_cmd(BHY2_CMD_SW_PASSTHROUGH, payload, payload_len, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
/* Read back the response */
rslt = bhy2_hif_get_status_fifo(&code, reg_data, length, actual_len, hif);
if (rslt == BHY2_OK)
{
/* Set back to previous mode */
tmp_buf = old_status;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
if (code != BHY2_STATUS_SW_PASS_THRU_RES)
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
}
}
}
return rslt;
}
int8_t bhy2_hif_get_post_mortem(uint16_t *code,
uint32_t *actual_len,
uint8_t *post_mortem,
uint32_t buffer_len,
struct bhy2_hif_dev *hif)
{
uint8_t old_status, tmp_buf;
int8_t rslt;
if (code != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
old_status = tmp_buf;
tmp_buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_exec_cmd(BHY2_CMD_REQ_POST_MORTEM_DATA, NULL, 0, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(code, post_mortem, buffer_len, actual_len, hif);
if (rslt == BHY2_OK)
{
tmp_buf = old_status;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
if (*code != BHY2_STATUS_CRASH_DUMP)
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_do_self_test(uint8_t phys_sensor_id,
struct bhy2_self_test_resp *self_test_resp,
struct bhy2_hif_dev *hif)
{
uint16_t code = 0;
uint8_t old_status, tmp_buf, req[4], ret_data[16];
uint32_t ret_len = 0;
int8_t rslt;
if (self_test_resp != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
old_status = tmp_buf;
tmp_buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
req[0] = phys_sensor_id;
req[1] = req[2] = req[3] = 0;
rslt = bhy2_hif_exec_cmd(BHY2_CMD_REQ_SELF_TEST, req, 4, hif);
if (rslt == BHY2_OK)
{
/* wait status ready */
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(&code, ret_data, 16, &ret_len, hif);
if (rslt == BHY2_OK)
{
tmp_buf = old_status;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
if (code != BHY2_STATUS_SELF_TEST_RES)
{
rslt = BHY2_E_TIMEOUT;
}
else
{
if (ret_data[0] != phys_sensor_id)
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
self_test_resp->test_status = ret_data[1];
self_test_resp->x_offset = BHY2_LE2S16(&ret_data[2]);
self_test_resp->y_offset = BHY2_LE2S16(&ret_data[4]);
self_test_resp->z_offset = BHY2_LE2S16(&ret_data[6]);
}
}
}
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_do_foc(uint8_t phys_sensor_id, struct bhy2_foc_resp *foc_resp, struct bhy2_hif_dev *hif)
{
uint16_t code = 0;
uint8_t old_status, tmp_buf, req[4], ret_data[16];
uint32_t ret_len = 0;
int8_t rslt;
if (foc_resp != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
old_status = tmp_buf;
tmp_buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
req[0] = phys_sensor_id;
req[1] = req[2] = req[3] = 0;
rslt = bhy2_hif_exec_cmd(BHY2_CMD_REQ_FOC, req, 4, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(&code, ret_data, 16, &ret_len, hif);
if (rslt == BHY2_OK)
{
tmp_buf = old_status;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
if (code != BHY2_STATUS_FOC_RES)
{
rslt = BHY2_E_TIMEOUT;
}
else
{
if (ret_data[0] != phys_sensor_id)
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
foc_resp->foc_status = ret_data[1];
foc_resp->x_offset = BHY2_LE2S16(&ret_data[2]);
foc_resp->y_offset = BHY2_LE2S16(&ret_data[4]);
foc_resp->z_offset = BHY2_LE2S16(&ret_data[6]);
}
}
}
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_sensor_ctrl(uint8_t sensor_id,
uint8_t ctrl_code,
uint8_t *payload,
uint32_t payload_len,
uint32_t *actual_len,
struct bhy2_hif_dev *hif)
{
uint16_t code = 0;
uint8_t old_status;
uint8_t tmp_buf[4] = {0};
uint8_t buf = 0;
uint32_t length;
uint16_t param_num;
int8_t rslt;
if ((payload != NULL) && (actual_len != NULL))
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &buf, 1, hif);
if (rslt == BHY2_OK)
{
old_status = buf;
buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &buf, 1, hif);
if (rslt == BHY2_OK)
{
/* Set the parameter to request data */
param_num = (uint16_t)(BHY2_PARAM_SET_SENSOR_CTRL | sensor_id);
tmp_buf[0] = ctrl_code | BHY2_PARAM_SENSOR_CTRL_READ;
length = 4;
rslt = bhy2_hif_set_parameter(param_num, tmp_buf, length, hif);
if (rslt == BHY2_OK)
{
/* Get parameter to read data */
param_num = (uint16_t)(BHY2_PARAM_GET_SENSOR_CTRL | sensor_id);
length = 0;
rslt = bhy2_hif_set_parameter(param_num, NULL, length, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(&code, payload, payload_len, actual_len, hif);
if (rslt == BHY2_OK)
{
buf = old_status;
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &buf, 1, hif);
if (rslt == BHY2_OK)
{
if (code != (BHY2_PARAM_SET_SENSOR_CTRL | sensor_id))
{
rslt = BHY2_E_INVALID_PARAM;
}
else if (payload[0] != ctrl_code)
{
rslt = BHY2_E_INVALID_PARAM;
}
}
}
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_set_sensor_ctrl(uint8_t sensor_id,
uint8_t ctrl_code,
const uint8_t *payload,
uint32_t payload_len,
struct bhy2_hif_dev *hif)
{
uint16_t param_num;
uint8_t command;
/* Set parameter to send data */
param_num = (uint16_t)(BHY2_PARAM_SET_SENSOR_CTRL | sensor_id);
command = (uint8_t)(ctrl_code & (~BHY2_PARAM_SENSOR_CTRL_READ));
return bhy2_hif_exec_cmd_generic(param_num, payload, payload_len, &command, 1, 0, hif);
}
int8_t bhy2_hif_get_bsx_state(uint16_t param_id,
uint8_t *bsx_state,
uint32_t state_len,
uint32_t *actual_len,
struct bhy2_hif_dev *hif)
{
uint8_t complete_flag = 0;
uint16_t tmp_state_len = 0;
uint8_t block_len = 0;
uint32_t read_len;
int8_t rslt;
uint8_t tmp_bsx_state_buf[BHY2_BSX_STATE_STRUCT_LEN];
uint16_t state_index = 0;
while (complete_flag == 0)
{
rslt = bhy2_hif_get_parameter(param_id, tmp_bsx_state_buf, BHY2_BSX_STATE_STRUCT_LEN, &read_len, hif);
if (rslt != BHY2_OK)
{
break;
}
if (read_len > BHY2_BSX_STATE_STRUCT_LEN)
{
rslt = BHY2_E_INVALID_PARAM - 50;
break;
}
complete_flag = tmp_bsx_state_buf[0] & BHY2_BSX_STATE_TRANSFER_COMPLETE;
block_len = tmp_bsx_state_buf[1];
tmp_state_len = BHY2_LE2U16(&tmp_bsx_state_buf[2]);
if (tmp_state_len == 0)
{
rslt = BHY2_E_INVALID_PARAM - 70;
break;
}
*actual_len = tmp_state_len; /* Report actual length, in case of invalid buffer */
if (state_len < tmp_state_len)
{
rslt = BHY2_E_BUFFER;
break;
}
memcpy(&bsx_state[state_index], &tmp_bsx_state_buf[4], tmp_bsx_state_buf[1]);
state_index += block_len;
}
return rslt;
}
int8_t bhy2_hif_set_bsx_state(uint16_t param_id, const uint8_t *bsx_state, uint32_t state_len, struct bhy2_hif_dev *hif)
{
uint32_t pos;
uint16_t num = 0;
int8_t rslt = BHY2_OK;
uint8_t tmp_bsx_state_buf[BHY2_BSX_STATE_STRUCT_LEN];
uint32_t state_buf_size;
for (pos = 0; pos < state_len; pos += BHY2_BSX_STATE_BLOCK_LEN, num++)
{
tmp_bsx_state_buf[0] = num & 0x7F;
state_buf_size = state_len - pos;
if (state_buf_size <= BHY2_BSX_STATE_BLOCK_LEN)
{
tmp_bsx_state_buf[0] |= 0x80;
tmp_bsx_state_buf[1] = (uint8_t)state_buf_size;
}
else
{
tmp_bsx_state_buf[1] = BHY2_BSX_STATE_BLOCK_LEN;
}
tmp_bsx_state_buf[2] = state_len & 0xFF;
tmp_bsx_state_buf[3] = (state_len >> 8) & 0xFF;
memcpy(&tmp_bsx_state_buf[4], &bsx_state[pos], tmp_bsx_state_buf[1]);
rslt = bhy2_hif_set_parameter(param_id, tmp_bsx_state_buf, BHY2_BSX_STATE_STRUCT_LEN, hif);
if (rslt != BHY2_OK)
{
break;
}
}
return rslt;
}
int8_t bhy2_hif_erase_flash(uint32_t start_addr, uint32_t end_addr, struct bhy2_hif_dev *hif)
{
uint8_t buf[8];
uint8_t int_status = 0;
uint16_t retry;
/* Variable to get the response code */
uint16_t code = 0;
int8_t rslt = BHY2_OK;
uint32_t ret_len;
/* Check for invalid end address */
if ((end_addr < start_addr) && (start_addr != BHY2_FLASH_BULK_ERASE))
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
/* Set start address */
buf[0] = (uint8_t)(start_addr & 0xFF);
buf[1] = (uint8_t)((start_addr >> 8) & 0xFF);
buf[2] = (uint8_t)((start_addr >> 16) & 0xFF);
buf[3] = (uint8_t)((start_addr >> 24) & 0xFF);
/* Set the end address */
buf[4] = (uint8_t)(end_addr & 0xFF);
buf[5] = (uint8_t)((end_addr >> 8) & 0xFF);
buf[6] = (uint8_t)((end_addr >> 16) & 0xFF);
buf[7] = (uint8_t)((end_addr >> 24) & 0xFF);
/* Erase flash */
rslt = bhy2_hif_exec_cmd(BHY2_CMD_ERASE_FLASH, buf, 8, hif);
if (rslt == BHY2_OK)
{
/* Check status and wait for at least 0.5 seconds for the erase to complete
* and time out provided for 10 Mins (1200 * 0.5 Seconds = 10 Min) */
for (retry = 0; retry < BHY2_QUERY_FLASH_MAX_RETRY; retry++)
{
rslt = bhy2_hif_get_interrupt_status(&int_status, hif);
if (rslt != BHY2_OK)
{
break;
}
if (int_status & BHY2_IST_MASK_STATUS)
{
break;
}
rslt = bhy2_hif_delay_us(500000, hif);
if (rslt != BHY2_OK)
{
break;
}
}
if (rslt == BHY2_OK)
{
/* Read status */
rslt = bhy2_hif_get_status_fifo(&code, NULL, 0, &ret_len, hif);
if (rslt == BHY2_OK)
{
/* Status code */
if (code != BHY2_STATUS_FLASH_ERASE_COMPLETE)
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
}
return rslt;
}
int8_t bhy2_hif_upload_to_flash(const uint8_t *firmware,
uint32_t length,
uint8_t *work_buffer,
uint32_t work_buf_len,
uint32_t *exp_size,
struct bhy2_hif_dev *hif,
bhy2_progress_callback progress_cb,
void *user_data)
{
int8_t rslt = BHY2_OK;
uint16_t magic;
uint32_t sector_addr = BHY2_FLASH_SECTOR_START_ADDR;
uint8_t addr_buf[4];
uint32_t remain, trans_len, pos;
uint16_t code;
if ((hif != NULL) && (firmware != NULL) && (work_buffer != NULL))
{
magic = BHY2_LE2U16(firmware);
if (magic != BHY2_FW_MAGIC)
{
rslt = BHY2_E_MAGIC;
}
else
{
pos = 0;
remain = length;
if (hif->read_write_len % 4)
{
hif->read_write_len = BHY2_ROUND_WORD_LOWER(hif->read_write_len);
}
while (remain > 0)
{
if (remain > hif->read_write_len)
{
trans_len = hif->read_write_len;
}
else
{
trans_len = remain;
}
/* Set the sector address */
addr_buf[0] = (uint8_t)(sector_addr & 0xFF);
addr_buf[1] = (uint8_t)((sector_addr >> 8) & 0xFF);
addr_buf[2] = (uint8_t)((sector_addr >> 16) & 0xFF);
addr_buf[3] = (uint8_t)((sector_addr >> 24) & 0xFF);
/* Transfer firmware to the flash */
rslt = bhy2_hif_exec_cmd_generic(BHY2_CMD_WRITE_FLASH, &firmware[pos], trans_len, addr_buf, 4, 0, hif);
if (rslt != BHY2_OK)
{
break;
}
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt != BHY2_OK)
{
break;
}
rslt = bhy2_hif_get_status_fifo(&code, work_buffer, work_buf_len, exp_size, hif);
if (rslt != BHY2_OK)
{
break;
}
/* Status code */
if (code != BHY2_STATUS_FLASH_WRITE_COMPLETE)
{
rslt = BHY2_E_TIMEOUT;
break;
}
pos += trans_len;
remain -= trans_len;
sector_addr += hif->read_write_len;
if (progress_cb != NULL) {
progress_cb(user_data, length, pos);
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_upload_to_flash_partly(const uint8_t *firmware,
uint32_t cur_pos,
uint32_t packet_len,
struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint16_t magic;
uint32_t sector_addr = BHY2_FLASH_SECTOR_START_ADDR + cur_pos;
uint8_t addr_buf[4];
uint16_t code;
uint8_t work_buffer[32];
uint32_t exp_size, work_buf_len = 32;
if ((hif != NULL) && (firmware != NULL))
{
if (cur_pos == 0)
{
magic = BHY2_LE2U16(firmware);
if (magic != BHY2_FW_MAGIC)
{
rslt = BHY2_E_MAGIC;
}
}
if (rslt == BHY2_OK)
{
/* Set the sector address */
addr_buf[0] = (uint8_t)(sector_addr & 0xFF);
addr_buf[1] = (uint8_t)((sector_addr >> 8) & 0xFF);
addr_buf[2] = (uint8_t)((sector_addr >> 16) & 0xFF);
addr_buf[3] = (uint8_t)((sector_addr >> 24) & 0xFF);
/* Transfer firmware to the flash */
rslt = bhy2_hif_exec_cmd_generic(BHY2_CMD_WRITE_FLASH, firmware, packet_len, addr_buf, 4, 0, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_wait_status_ready(hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_get_status_fifo(&code, work_buffer, work_buf_len, &exp_size, hif);
if (rslt == BHY2_OK)
{
if (code != BHY2_STATUS_FLASH_WRITE_COMPLETE)
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_boot_from_flash(struct bhy2_hif_dev *hif)
{
int8_t rslt;
rslt = bhy2_hif_exec_cmd(BHY2_CMD_BOOT_FLASH, NULL, 0, hif);
if (rslt == BHY2_OK)
{
rslt = bhy2_hif_check_boot_status_flash(hif);
}
return rslt;
}
int8_t bhy2_hif_request_hw_timestamp(struct bhy2_hif_dev *hif)
{
uint8_t time_ev_req = 1;
return bhy2_hif_set_regs(BHY2_REG_TIME_EV_REQ, &time_ev_req, 1, hif);
}
int8_t bhy2_hif_get_hw_timestamp(uint64_t *ts_ticks, struct bhy2_hif_dev *hif)
{
int8_t rslt = BHY2_OK;
uint8_t ts_regs[5];
if (ts_ticks != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTR_TIME_0, ts_regs, 5, hif);
if (rslt == BHY2_OK)
{
*ts_ticks = BHY2_LE2U40(ts_regs);
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_req_and_get_hw_timestamp(uint64_t *ts_ticks, struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint8_t retry;
uint8_t tmp_buf[5];
uint64_t ts_old;
if (ts_ticks != NULL)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTR_TIME_0, tmp_buf, 5, hif);
if (rslt == BHY2_OK)
{
ts_old = BHY2_LE2U40(tmp_buf);
tmp_buf[0] = 1;
rslt = bhy2_hif_set_regs(BHY2_REG_TIME_EV_REQ, tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
for (retry = 0; retry < 5; retry++)
{
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTR_TIME_0, tmp_buf, 5, hif);
if (rslt == BHY2_OK)
{
*ts_ticks = BHY2_LE2U40(tmp_buf);
if (*ts_ticks != ts_old)
{
break;
}
rslt = bhy2_hif_delay_us(10, hif);
if (rslt != BHY2_OK)
{
break;
}
}
}
if ((retry >= 5) && (*ts_ticks == ts_old))
{
rslt = BHY2_E_TIMEOUT;
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_sensor_info(uint8_t sensor_id, struct bhy2_sensor_info *info, struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint32_t length;
uint8_t bytes[28];
if (info != NULL)
{
/* Checking for a valid sensor ID */
if (sensor_id > BHY2_SPECIAL_SENSOR_ID_OFFSET)
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
rslt = bhy2_hif_get_parameter((uint16_t)(BHY2_PARAM_SENSOR_INFO_0 + sensor_id), bytes, 28, &length, hif);
if (rslt == BHY2_OK)
{
if (length != 28)
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
info->sensor_type = bytes[0];
info->driver_id = bytes[1];
info->driver_version = bytes[2];
info->power = bytes[3];
info->max_range.bytes[0] = bytes[4];
info->max_range.bytes[1] = bytes[5];
info->resolution.bytes[0] = bytes[6];
info->resolution.bytes[1] = bytes[7];
info->max_rate.bytes[0] = bytes[8];
info->max_rate.bytes[1] = bytes[9];
info->max_rate.bytes[2] = bytes[10];
info->max_rate.bytes[3] = bytes[11];
info->fifo_reserved.bytes[0] = bytes[12];
info->fifo_reserved.bytes[1] = bytes[13];
info->fifo_reserved.bytes[2] = bytes[14];
info->fifo_reserved.bytes[3] = bytes[15];
info->fifo_max.bytes[0] = bytes[16];
info->fifo_max.bytes[1] = bytes[17];
info->fifo_max.bytes[2] = bytes[18];
info->fifo_max.bytes[3] = bytes[19];
info->event_size = bytes[20];
info->min_rate.bytes[0] = bytes[21];
info->min_rate.bytes[1] = bytes[22];
info->min_rate.bytes[2] = bytes[23];
info->min_rate.bytes[3] = bytes[24];
info->max_range.u16_val = BHY2_LE2U16(info->max_range.bytes);
info->resolution.u16_val = BHY2_LE2U16(info->resolution.bytes);
info->max_rate.u32_val = BHY2_LE2U32(info->max_rate.bytes);
info->fifo_reserved.u32_val = BHY2_LE2U32(info->fifo_reserved.bytes);
info->fifo_max.u32_val = BHY2_LE2U32(info->fifo_max.bytes);
info->min_rate.u32_val = BHY2_LE2U32(info->min_rate.bytes);
}
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_get_phys_sensor_info(uint8_t sensor_id, struct bhy2_phys_sensor_info *info, struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint32_t length;
uint8_t bytes[20];
if (info != NULL)
{
rslt = bhy2_hif_get_parameter((uint16_t)(BHY2_PARAM_PHYSICAL_SENSOR_BASE + sensor_id), bytes, 20, &length, hif);
if (rslt == BHY2_OK)
{
if (length != 20)
{
rslt = BHY2_E_INVALID_PARAM;
}
else
{
info->sensor_type = bytes[0];
info->driver_id = bytes[1];
info->driver_version = bytes[2];
info->power_current = bytes[3];
info->curr_range.bytes[0] = bytes[4];
info->curr_range.bytes[1] = bytes[5];
info->flags = bytes[6];
info->slave_address = bytes[7];
info->gpio_assignment = bytes[8];
info->curr_rate.bytes[0] = bytes[9];
info->curr_rate.bytes[1] = bytes[10];
info->curr_rate.bytes[2] = bytes[11];
info->curr_rate.bytes[3] = bytes[12];
info->num_axis = bytes[13];
info->orientation_matrix[0] = bytes[14];
info->orientation_matrix[1] = bytes[15];
info->orientation_matrix[2] = bytes[16];
info->orientation_matrix[3] = bytes[17];
info->orientation_matrix[4] = bytes[18];
info->curr_range.u16_val = BHY2_LE2U16(info->curr_range.bytes);
info->curr_rate.u32_val = BHY2_LE2U32(info->curr_rate.bytes);
}
}
}
else
{
rslt = BHY2_E_NULL_PTR;
}
return rslt;
}
int8_t bhy2_hif_set_orientation_matrix(uint8_t sensor_id,
struct bhy2_orient_matrix orient_matrix,
struct bhy2_hif_dev *hif)
{
int8_t rslt;
uint8_t tmp_matrix_buf[8]; /* Includes 2 reserved bytes */
memset(tmp_matrix_buf, 0, sizeof(tmp_matrix_buf));
tmp_matrix_buf[0] = BHY2_BYTE_TO_NIBBLE(&orient_matrix.c[0]);
tmp_matrix_buf[1] = BHY2_BYTE_TO_NIBBLE(&orient_matrix.c[2]);
tmp_matrix_buf[2] = BHY2_BYTE_TO_NIBBLE(&orient_matrix.c[4]);
tmp_matrix_buf[3] = BHY2_BYTE_TO_NIBBLE(&orient_matrix.c[6]);
tmp_matrix_buf[4] = orient_matrix.c[8] & 0x0F;
rslt = bhy2_hif_set_parameter((uint16_t)(BHY2_PARAM_PHYSICAL_SENSOR_BASE + sensor_id), tmp_matrix_buf, 8, hif);
return rslt;
}
/*lint -e{715} suppressed parameters not referenced info as all the calling functions are parsing the valid values to
* all parameters*/
int8_t bhy2_hif_set_inject_data_mode(const uint8_t *payload,
uint8_t payload_len,
const uint8_t *work_buf,
uint32_t work_buf_len,
const uint32_t *actual_len,
struct bhy2_hif_dev *hif)
{
uint8_t tmp_buf;
int8_t rslt;
rslt = bhy2_hif_get_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
tmp_buf &= (uint8_t)(~(BHY2_HIF_CTRL_ASYNC_STATUS_CHANNEL));
rslt = bhy2_hif_set_regs(BHY2_REG_HOST_INTERFACE_CTRL, &tmp_buf, 1, hif);
if (rslt == BHY2_OK)
{
/* Set injection mode */
rslt = bhy2_hif_set_regs(BHY2_REG_CHAN_CMD, payload, payload_len, hif);
if (rslt != BHY2_OK)
{
rslt = BHY2_E_NULL_PTR;
}
}
}
return rslt;
}
int8_t bhy2_hif_inject_data(const uint8_t *payload, uint32_t payload_len, struct bhy2_hif_dev *hif)
{
return bhy2_hif_exec_cmd(BHY2_CMD_INJECT_DATA, payload, payload_len, hif);
}
Reference in New Issue Copy Permalink