Skip to content

Commit

Permalink
[update] update lhal:adc,i2c,usb_v2, and soc for coredump
Browse files Browse the repository at this point in the history
  • Loading branch information
sakumisue committed Jul 26, 2023
1 parent 06b3131 commit 0a6e1a9
Show file tree
Hide file tree
Showing 13 changed files with 168 additions and 42 deletions.
6 changes: 0 additions & 6 deletions drivers/lhal/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,12 +1,6 @@
sdk_generate_library()

if(NOT CONFIG_ROMAPI)
sdk_library_add_sources(src/bflb_common.c)
else()
if((NOT ("${CHIP}" STREQUAL "bl602")) AND (NOT ("${CHIP}" STREQUAL "bl702")))
sdk_library_add_sources(src/bflb_common.c)
endif()
endif()

sdk_library_add_sources(
src/bflb_adc.c
Expand Down
10 changes: 10 additions & 0 deletions drivers/lhal/include/bflb_i2c.h
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,16 @@
* @}
*/

/** @defgroup I2C_CMD i2c feature control cmd definition
* @{
*/
#define I2C_CMD_SET_SCL_SYNC (0x01) /* Enable or disable multi-master and clock-stretching */
#define I2C_CMD_SET_DEGLITCH (0x02) /* 0 for disable deglitch, others for deglitch count */

/**
* @}
*/

/**
* @brief I2C message structure
*
Expand Down
15 changes: 12 additions & 3 deletions drivers/lhal/src/bflb_adc.c
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,11 @@ void bflb_adc_init(struct bflb_device_s *dev, const struct bflb_adc_config_s *co
regval |= (1 << AON_GPADC_V11_SEL_SHIFT); /* V11 select 1.1V */
regval |= (config->clk_div << AON_GPADC_CLK_DIV_RATIO_SHIFT); /* clock div */
regval |= (config->resolution << AON_GPADC_RES_SEL_SHIFT); /* resolution */
#if defined(BL702L)
regval |= AON_GPADC_LOWV_DET_EN; /* low voltage detect enable */
regval |= AON_GPADC_VCM_HYST_SEL; /* VCM hyst select */
regval |= AON_GPADC_VCM_SEL_EN; /* VCM select enable */
#endif
if (config->scan_conv_mode) {
regval |= AON_GPADC_SCAN_EN;
regval |= AON_GPADC_CLK_ANA_INV;
Expand All @@ -92,10 +97,14 @@ void bflb_adc_init(struct bflb_device_s *dev, const struct bflb_adc_config_s *co
regval |= (2 << AON_GPADC_DLY_SEL_SHIFT);
regval |= (2 << AON_GPADC_CHOP_MODE_SHIFT); /* Vref AZ and PGA chop on */
regval |= (1 << AON_GPADC_PGA1_GAIN_SHIFT); /* gain 1 */
regval |= (1 << AON_GPADC_PGA2_GAIN_SHIFT); /* gain 1 */
#if defined(BL702L)
regval &= ~AON_GPADC_PGA2_GAIN_MASK; /* gain 2 */
#else
regval |= (1 << AON_GPADC_PGA2_GAIN_SHIFT); /* gain 2 */
#endif
regval |= AON_GPADC_PGA_EN;
regval |= (8 << AON_GPADC_PGA_OS_CAL_SHIFT);
regval |= (1 << AON_GPADC_PGA_VCM_SHIFT); /* PGA output common mode control 1.4V */
regval |= (1 << AON_GPADC_PGA_VCM_SHIFT); /* PGA output common mode control 1.2V */

if (config->vref == ADC_VREF_2P0V) {
regval |= AON_GPADC_VREF_SEL;
Expand Down Expand Up @@ -682,4 +691,4 @@ void bflb_adc_vbat_disable(struct bflb_device_s *dev)
regval = getreg32(reg_base + AON_GPADC_REG_CONFIG2_OFFSET);
regval &= ~AON_GPADC_VBAT_EN;
putreg32(regval, reg_base + AON_GPADC_REG_CONFIG2_OFFSET);
}
}
22 changes: 10 additions & 12 deletions drivers/lhal/src/bflb_common.c
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include "bflb_common.h"
#include "bflb_core.h"

void *ATTR_TCM_SECTION arch_memcpy(void *dst, const void *src, uint32_t n)
__WEAK void *ATTR_TCM_SECTION arch_memcpy(void *dst, const void *src, uint32_t n)
{
const uint8_t *p = src;
uint8_t *q = dst;
Expand All @@ -13,7 +13,7 @@ void *ATTR_TCM_SECTION arch_memcpy(void *dst, const void *src, uint32_t n)
return dst;
}

uint32_t *ATTR_TCM_SECTION arch_memcpy4(uint32_t *dst, const uint32_t *src, uint32_t n)
__WEAK uint32_t *ATTR_TCM_SECTION arch_memcpy4(uint32_t *dst, const uint32_t *src, uint32_t n)
{
const uint32_t *p = src;
uint32_t *q = dst;
Expand All @@ -25,7 +25,7 @@ uint32_t *ATTR_TCM_SECTION arch_memcpy4(uint32_t *dst, const uint32_t *src, uint
return dst;
}

void *ATTR_TCM_SECTION arch_memcpy_fast(void *pdst, const void *psrc, uint32_t n)
__WEAK void *ATTR_TCM_SECTION arch_memcpy_fast(void *pdst, const void *psrc, uint32_t n)
{
uint32_t left, done, i = 0;
uint8_t *dst = (uint8_t *)pdst;
Expand All @@ -47,7 +47,7 @@ void *ATTR_TCM_SECTION arch_memcpy_fast(void *pdst, const void *psrc, uint32_t n
return dst;
}

void *ATTR_TCM_SECTION arch_memset(void *s, uint8_t c, uint32_t n)
__WEAK void *ATTR_TCM_SECTION arch_memset(void *s, uint8_t c, uint32_t n)
{
uint8_t *p = (uint8_t *)s;

Expand All @@ -59,7 +59,7 @@ void *ATTR_TCM_SECTION arch_memset(void *s, uint8_t c, uint32_t n)
return s;
}

uint32_t *ATTR_TCM_SECTION arch_memset4(uint32_t *dst, const uint32_t val, uint32_t n)
__WEAK uint32_t *ATTR_TCM_SECTION arch_memset4(uint32_t *dst, const uint32_t val, uint32_t n)
{
uint32_t *q = dst;

Expand All @@ -70,7 +70,7 @@ uint32_t *ATTR_TCM_SECTION arch_memset4(uint32_t *dst, const uint32_t val, uint3
return dst;
}

int ATTR_TCM_SECTION arch_memcmp(const void *s1, const void *s2, uint32_t n)
__WEAK int ATTR_TCM_SECTION arch_memcmp(const void *s1, const void *s2, uint32_t n)
{
const unsigned char *c1 = s1, *c2 = s2;
int d = 0;
Expand Down Expand Up @@ -110,9 +110,9 @@ void *bflb_get_no_cache_addr(const void *addr)
return (void *)((a & ~0xF0000000UL) | 0x20000000UL);
}
// pSRAM
// if ((a & 0xF0000000UL) == 0xA0000000UL) {
// return (void *)((a & ~0xF0000000UL) | 0x10000000UL);
// }
if ((a & 0xF0000000UL) == 0xA0000000UL) {
return (void *)((a & ~0xF0000000UL) | 0x10000000UL);
}

return NULL;
}
Expand Down Expand Up @@ -305,9 +305,7 @@ uint32_t ATTR_TCM_SECTION bflb_soft_crc32_ex(uint32_t initial, void *in, uint32_
return ~crc;
}

#if !defined(BL602) && !defined(BL702)
uint32_t ATTR_TCM_SECTION bflb_soft_crc32(void *in, uint32_t len)
__WEAK uint32_t ATTR_TCM_SECTION bflb_soft_crc32(void *in, uint32_t len)
{
return bflb_soft_crc32_ex(0, in, len);
}
#endif
65 changes: 49 additions & 16 deletions drivers/lhal/src/bflb_i2c.c
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@ static void bflb_i2c_addr_config(struct bflb_device_s *dev, uint16_t slaveaddr,
regval &= ~I2C_CR_I2C_10B_ADDR_EN;
}
#endif
regval &= ~I2C_CR_I2C_SCL_SYNC_EN;
putreg32(subaddr, reg_base + I2C_SUB_ADDR_OFFSET);
putreg32(regval, reg_base + I2C_CONFIG_OFFSET);
}
Expand Down Expand Up @@ -75,31 +74,41 @@ static void bflb_i2c_set_frequence(struct bflb_device_s *dev, uint32_t freq)
{
uint32_t regval;
uint32_t reg_base;
uint32_t phase;
uint32_t tmp;
uint32_t phase, phase0, phase1, phase2, phase3;
uint32_t bias;

reg_base = dev->reg_base;

phase = bflb_clk_get_peripheral_clock(BFLB_DEVICE_TYPE_I2C, dev->idx) / (freq * 4) - 1;
phase = (bflb_clk_get_peripheral_clock(BFLB_DEVICE_TYPE_I2C, dev->idx) + freq / 2) / freq - 4;
phase0 = (phase + 4) / 8;
phase2 = (phase * 3 + 4) / 8;
phase3 = (phase + 4) / 8;
phase1 = phase - (phase0 + phase2 + phase3);
regval = getreg32(reg_base + I2C_CONFIG_OFFSET);

if ((regval & I2C_CR_I2C_DEG_EN) && (regval & I2C_CR_I2C_SCL_SYNC_EN)) {
bias = (regval & I2C_CR_I2C_DEG_CNT_MASK) >> I2C_CR_I2C_DEG_CNT_SHIFT;
bias += 1;
} else {
bias = 0;
}
if (regval & I2C_CR_I2C_SCL_SYNC_EN) {
bias += 3;
}

if (freq > 100000) {
tmp = ((phase / 4) / 0.5);
if (phase1 < (bias + 1)) {
phase1 = 1;
} else {
tmp = (phase / 4);
phase1 -= bias;
}

regval = (phase - tmp) << I2C_CR_I2C_PRD_S_PH_0_SHIFT;
regval |= (phase + tmp) << I2C_CR_I2C_PRD_S_PH_1_SHIFT;
regval |= (phase) << I2C_CR_I2C_PRD_S_PH_2_SHIFT;
regval |= (phase) << I2C_CR_I2C_PRD_S_PH_3_SHIFT;
regval = phase0 << I2C_CR_I2C_PRD_S_PH_0_SHIFT;
regval |= phase1 << I2C_CR_I2C_PRD_S_PH_1_SHIFT;
regval |= phase2 << I2C_CR_I2C_PRD_S_PH_2_SHIFT;
regval |= phase3 << I2C_CR_I2C_PRD_S_PH_3_SHIFT;

putreg32(regval, reg_base + I2C_PRD_START_OFFSET);
putreg32(regval, reg_base + I2C_PRD_STOP_OFFSET);

regval = (phase - tmp) << I2C_CR_I2C_PRD_D_PH_0_SHIFT;
regval |= (phase + tmp) << I2C_CR_I2C_PRD_D_PH_1_SHIFT;
regval |= (phase + tmp) << I2C_CR_I2C_PRD_D_PH_2_SHIFT;
regval |= (phase - tmp) << I2C_CR_I2C_PRD_D_PH_3_SHIFT;
putreg32(regval, reg_base + I2C_PRD_DATA_OFFSET);
}

Expand Down Expand Up @@ -475,7 +484,31 @@ uint32_t bflb_i2c_get_intstatus(struct bflb_device_s *dev)
int bflb_i2c_feature_control(struct bflb_device_s *dev, int cmd, size_t arg)
{
int ret = 0;
uint32_t reg_base;
uint32_t regval;

reg_base = dev->reg_base;
switch (cmd) {
case I2C_CMD_SET_SCL_SYNC:
regval = getreg32(reg_base + I2C_CONFIG_OFFSET);
if (arg == 0) {
regval &= ~I2C_CR_I2C_SCL_SYNC_EN;
} else {
regval |= I2C_CR_I2C_SCL_SYNC_EN;
}
putreg32(regval, reg_base + I2C_CONFIG_OFFSET);
break;
case I2C_CMD_SET_DEGLITCH:
regval = getreg32(reg_base + I2C_CONFIG_OFFSET);
regval &= ~I2C_CR_I2C_DEG_CNT_MASK;
if (arg == 0) {
regval &= ~I2C_CR_I2C_DEG_EN;
} else {
regval |= I2C_CR_I2C_DEG_EN;
regval |= ((arg - 1) << I2C_CR_I2C_DEG_CNT_SHIFT);
}
putreg32(regval, reg_base + I2C_CONFIG_OFFSET);
break;
default:
ret = -EPERM;
break;
Expand Down
4 changes: 2 additions & 2 deletions drivers/lhal/src/bflb_usb_v2.c
Original file line number Diff line number Diff line change
Expand Up @@ -927,13 +927,13 @@ void USBD_IRQHandler(int irq, void *arg)

if (subgroup_intstatus & USB_SUSP_INT) {
bflb_usb_source_group_int_clear(2, USB_SUSP_INT);

bflb_usb_reset_fifo(USB_FIFO_F0);
bflb_usb_reset_fifo(USB_FIFO_F1);
bflb_usb_reset_fifo(USB_FIFO_F2);
bflb_usb_reset_fifo(USB_FIFO_F3);
bflb_usb_reset_fifo(USB_FIFO_CXF);

memset(&g_bl_udc, 0, sizeof(g_bl_udc));
usbd_event_suspend_handler();
}
if (subgroup_intstatus & USB_RESM_INT) {
Expand Down Expand Up @@ -1107,4 +1107,4 @@ void usbd_execute_test_mode(struct usb_setup_packet *setup)
break;
}
}
#endif
#endif
11 changes: 10 additions & 1 deletion drivers/soc/bl602/std/startup/interrupt.c
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,15 @@ void exception_entry(void)
WRITE_CSR(CSR_MEPC, epc);
} else {
while (1) {
#ifdef CONFIG_COREDUMP
/* For stack check */
extern uintptr_t __freertos_irq_stack_top;

/* XXX change sp to irq stack base */
__asm__ volatile("add sp, x0, %0" ::"r"(&__freertos_irq_stack_top));
void bl_coredump_run(void);
bl_coredump_run();
#endif
}
}
}
Expand Down Expand Up @@ -201,4 +210,4 @@ __attribute__((interrupt)) __attribute__((weak)) void default_interrupt_handler(
__asm volatile("csrw mepc,a1");
__asm volatile("csrw mcause,a0");
__asm volatile("addi sp,sp,8");
}
}
2 changes: 2 additions & 0 deletions drivers/soc/bl616/std/include/bl616_hbn.h
Original file line number Diff line number Diff line change
Expand Up @@ -679,6 +679,8 @@ BL_Err_Type HBN_Power_Off_RC32K(void);
BL_Err_Type HBN_Trim_Ldo33VoutTrim(void);
BL_Err_Type HBN_Trim_RC32K(void);
BL_Err_Type HBN_Set_BOD_Cfg(HBN_BOD_CFG_Type *cfg);
void HBN_Get_Reset_Event(uint8_t* event);
void HBN_Clr_Reset_Event(void);
BL_Err_Type HBN_Clear_RTC_INT(void);
/*----------*/

Expand Down
30 changes: 30 additions & 0 deletions drivers/soc/bl616/std/src/bl616_hbn.c
Original file line number Diff line number Diff line change
Expand Up @@ -555,6 +555,35 @@ BL_Err_Type HBN_Set_BOD_Config(uint8_t enable, HBN_BOD_THRES_Type threshold, HBN
return SUCCESS;
}

/****************************************************************************/ /**
* @brief HBN get reset event
*
* @param[out] event
* [4] : bor_out_ event
* [3] : pwr_rst_n event
* [2] : sw_rst event
* [1] : por_out event
* [0] : watch dog reset
*
*******************************************************************************/
void HBN_Get_Reset_Event(uint8_t* event)
{
uint32_t tmpVal;
tmpVal = BL_RD_REG(HBN_BASE, HBN_GLB);
*event = BL_GET_REG_BITS_VAL(tmpVal,HBN_RESET_EVENT);
}
/****************************************************************************/ /**
* @brief HBN clear reset event
*
*******************************************************************************/
void HBN_Clr_Reset_Event(void)
{
uint32_t tmpVal;
tmpVal = BL_RD_REG(HBN_BASE, HBN_GLB);
tmpVal |= (1<<13);
BL_WR_REG(HBN_BASE, HBN_GLB, tmpVal);
}

/****************************************************************************/ /**
* @brief HBN set ldo11aon voltage out
*
Expand Down Expand Up @@ -2000,6 +2029,7 @@ BL_Err_Type HBN_Disable_BOD_IRQ(void)
return SUCCESS;
}


/****************************************************************************/ /**
* @brief HBN out0 install interrupt callback
*
Expand Down
9 changes: 9 additions & 0 deletions drivers/soc/bl616/std/startup/interrupt.c
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,15 @@ void exception_entry(uintptr_t *regs)
WRITE_CSR(CSR_MEPC, epc);
} else {
while (1) {
#ifdef CONFIG_COREDUMP
/* For stack check */
extern uintptr_t __freertos_irq_stack_top;

/* XXX change sp to irq stack base */
__asm__ volatile("add sp, x0, %0" ::"r"(&__freertos_irq_stack_top));
void bl_coredump_run(void);
bl_coredump_run();
#endif
}
}
}
Expand Down
16 changes: 15 additions & 1 deletion drivers/soc/bl616/std/startup/system_bl616.c
Original file line number Diff line number Diff line change
Expand Up @@ -164,9 +164,23 @@ void SystemInit(void)
BL_WR_REG(GLB_BASE, GLB_UART_CFG1, 0xffffffff);
BL_WR_REG(GLB_BASE, GLB_UART_CFG2, 0x0000ffff);

extern uint8_t __LD_CONFIG_EM_SEL;
volatile uint32_t em_size;

em_size = (uint32_t)&__LD_CONFIG_EM_SEL;

uint32_t tmpVal = 0;
tmpVal = BL_RD_REG(GLB_BASE, GLB_SRAM_CFG3);
tmpVal = BL_SET_REG_BITS_VAL(tmpVal, GLB_EM_SEL, 0x00);

if (em_size == 0) {
tmpVal = BL_SET_REG_BITS_VAL(tmpVal, GLB_EM_SEL, GLB_WRAM160KB_EM0KB);
} else if (em_size == 32 * 1024) {
tmpVal = BL_SET_REG_BITS_VAL(tmpVal, GLB_EM_SEL, GLB_WRAM128KB_EM32KB);
} else if (em_size == 64 * 1024) {
tmpVal = BL_SET_REG_BITS_VAL(tmpVal, GLB_EM_SEL, GLB_WRAM96KB_EM64KB);
} else {
tmpVal = BL_SET_REG_BITS_VAL(tmpVal, GLB_EM_SEL, GLB_WRAM160KB_EM0KB);
}
BL_WR_REG(GLB_BASE, GLB_SRAM_CFG3, tmpVal);
}

Expand Down
Loading

0 comments on commit 0a6e1a9

Please sign in to comment.