L9779: update driver (#3898)

* L9779: update driver

* L9779: missed chip_init

* L9779: signature
This commit is contained in:
Andrey G 2022-02-06 19:39:37 +03:00 committed by GitHub
parent af3b66185a
commit c53fd0eaf9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 480 additions and 12 deletions

View File

@ -2,6 +2,29 @@
* l9779.cpp
*
* Created on: Jan 10, 2022
*
* Andrey Gusakov, (c) 2022
*
* Masks/inputs bits:
* 0..3 - IGN1 .. 4 - Ignition pre-drivers
* Driven by logical-AND of SPI control bit and dedicated parallel input IGNI1...IGNI4
* 4..7 - OUT1 .. 4 - Protected low-side drivers with max current 2.2A
* Driven by logical-AND of SPI control bit and dedicated parallel input IN1...IN4
* 8 - OUT5 - Protected low-side driver with max current 3A
* Driven by logical-AND of SPI control bit and dedicated parallel input IN5
* 9..10 - OUT6,7 - Protected low-side drivers with max current 5A (O2 heaters)
* Driven by logical-AND of SPI control bit and dedicated parallel input IN6, IN7.
* 11 - Unused (no OUT8), IN8-PWM is used for stepper
* 12..15 - OUT9..12 - Not exist on L9779WD-SPI, TODO: check L9779WD
* 16..17 - OUT13..14 - Protected low side relay drivers with max current 600 mA and Low Battery Volatage function
* 18..21 - OUT15..18 - Protected low side relay drivers with max current 600 mA
* 22 - Unused (no OUT19)
* 23 - OUT20 - Protected low side low current driver with max current 50 mA
* 24..27 - OUTA..D - Configurable outputs (OD, PP) with max current 0.6 A (for low and high side FETs)
* Can be configured for stepper motor driving.
* Stepper is controlled by the logic AND between PWM (IN8) input pin and PWM SPI bit.
* 28..31 - OUT25..27 - Unused on L9779WD-SPI, TODO for L9779WD
* 32 - MR - Main Relay low side driver with max current 0.6 A, automaticly controlled
*/
#include "pch.h"
@ -23,6 +46,15 @@
/*==========================================================================*/
#define DRIVER_NAME "l9779"
#define DIAG_PERIOD_MS (7)
typedef enum {
L9779_DISABLED = 0,
L9779_WAIT_INIT,
L9779_READY,
L9779_FAILED
} l9779_drv_state;
/* SPI communication helpers */
/* Out frame */
/* D0 - parity */
@ -35,6 +67,12 @@
#define MSG_SET_ADDR(a) (((a) & 0x1f) << 10)
/* D15 - x */
/* ADD user for read commands */
#define MSG_READ_ADDR (0x10)
#define MSG_W(a, d) (static_cast<uint16_t>((MSG_SET_ADDR(a) | MSG_SET_DATA(d))))
#define MSG_R(a) (static_cast<uint16_t>((MSG_SET_ADDR(MSG_READ_ADDR) | MSG_SET_SUBADDR(d))))
/* In frame */
/* D0 - parity */
#define MSG_GET_PARITY(rx) (((rx) >> 0) & 0x01)
@ -50,6 +88,16 @@
/* register address that never can be replyed */
#define REG_INVALID 0xff
/* Write only registers */
#define CMD_CLOCK_UNLOCK_SW_RST(d) MSG_W(0x0c, (d))
#define CMD_START_REACT(d) MSG_W(0x0d, (d))
#define CMD_CONTR_REG(n, d) MSG_W(0x08 + (n), (d))
/* Read only registers */
/* IGN1..4 + OUT1..7 */
#define OUT_DIRECT_DRIVE_MASK 0x7ff
/*==========================================================================*/
/* Driver exported variables. */
/*==========================================================================*/
@ -71,20 +119,46 @@ struct L9779 : public GpioChip {
bool spi_parity_odd(uint16_t x);
int spi_validate(uint16_t rx);
int spi_rw(uint16_t tx, uint16_t *rx_ptr);
int spi_rw_array(const uint16_t *tx, uint16_t *rx, int n);
int update_output();
int update_direct_output(size_t pin, int value);
int wake_driver();
int chip_reset();
int chip_init_data();
int chip_init();
brain_pin_diag_e getOutputDiag(size_t pin);
brain_pin_diag_e getInputDiag(size_t pin);
const l9779_config *cfg;
/* thread stuff */
thread_t *thread;
THD_WORKING_AREA(thread_wa, 256);
semaphore_t wake;
/* state to be sent to chip */
uint32_t o_state;
/* output enabled mask */
uint32_t o_oe_mask;
/* cached output registers state - value last send to chip */
uint32_t o_data_cached;
l9779_drv_state drv_state;
/* last accessed register, for validation on next SPI access */
uint8_t last_reg;
/* chip needs reintialization due to some critical issue */
bool need_init;
/* statistic */
//int por_cnt;
//int wdr_cnt;
//int comfe_cnt;
//int init_cnt;
int init_cnt;
//int init_req_cnt;
int spi_cnt;
int spi_err_parity; /* parity errors in rx data */
@ -104,7 +178,7 @@ static const char* l9779_pin_names[L9779_SIGNALS] = {
"L9779.OUT13", "L9779.OUT14", "L9779.OUT15", "L9779.OUT16",
"L9779.OUT17", "L9779.OUT18", "L9779.OUT19", "L9779.OUT20",
"L9779.OUTA", "L9779.OUTB", "L9779.OUTC", "L9779.OUTD",
"L9779.OUT21", "L9779.OUT22", "L9779.OUT23", "L9779.OUT24",
"L9779.OUT25", "L9779.OUT26", "L9779.OUT27", "L9779.OUT28",
"L9779.MRD", "L9779.KEY"
};
@ -192,6 +266,265 @@ int L9779::spi_rw(uint16_t tx, uint16_t *rx_ptr)
/* no errors for now */
return ret;
}
/**
* @return -1 in case of communication error
*/
int L9779::spi_rw_array(const uint16_t *tx, uint16_t *rx, int n)
{
int ret;
uint16_t rxdata;
SPIDriver *spi = cfg->spi_bus;
/**
* 15.1 SPI Protocol
*
* after a read or write command: the address and content of the selected register
* is transmitted with the next SPI transmission (for not existing addresses or
* wrong access mode the data is always 0)
*/
/* Acquire ownership of the bus. */
spiAcquireBus(spi);
/* Setup transfer parameters. */
spiStart(spi, &cfg->spi_config);
for (int i = 0; i < n; i++) {
/* Slave Select assertion. */
spiSelect(spi);
/* data transfer */
rxdata = spiPolledExchange(spi, tx[i]);
if (rx)
rx[i] = rxdata;
/* Slave Select de-assertion. */
spiUnselect(spi);
/* statistic and debug */
this->tx = tx[i];
this->rx = rxdata;
this->spi_cnt++;
/* validate reply and save last accessed register */
ret = spi_validate(rxdata);
last_reg = getRegisterFromResponse(tx[i]);
if (ret < 0)
break;
}
/* Ownership release. */
spiReleaseBus(spi);
/* no errors for now */
return ret;
}
/* use datasheet numbering, starting from 1, skip 4 ignition channels */
#define OUT_ENABLED(n) (!!(o_state & BIT((n) + L9779_OUTPUTS_IGN - 1)))
#define SHIFT_N_OUT_TO_M(n, m) (OUT_ENABLED(n) << (m))
/* use datasheet numbering, starting from 1 */
#define IGN_ENABLED(n) (!!(o_state & BIT((n) - 1)))
#define SHIFT_N_IGN_TO_M(n, m) (IGN_ENABLED(n) << (m))
int L9779::update_output()
{
int ret;
uint8_t regs[4];
/* set value only for non-direct driven pins */
uint32_t o_data = o_state & ~OUT_DIRECT_DRIVE_MASK;
/* direct driven outputs are logicaly-AND spi bit and dedicated input
* set bits to all enabled direct driven outputs */
o_data = o_state | (o_oe_mask & OUT_DIRECT_DRIVE_MASK);
/* nightmare... briliant mapping */
regs[0] =
SHIFT_N_OUT_TO_M( 1, 7) | /* bit 7 - OUT1 */
SHIFT_N_OUT_TO_M( 2, 6) | /* and so on, refer to datasheet */
SHIFT_N_OUT_TO_M( 3, 5) |
SHIFT_N_OUT_TO_M( 4, 4) |
SHIFT_N_OUT_TO_M( 5, 3) |
SHIFT_N_OUT_TO_M(20, 2);
regs[1] =
SHIFT_N_OUT_TO_M(15, 7) |
SHIFT_N_OUT_TO_M(14, 6) |
/* reserved + don't care */
SHIFT_N_IGN_TO_M( 1, 3) |
SHIFT_N_IGN_TO_M( 2, 2) |
SHIFT_N_IGN_TO_M( 3, 1) |
SHIFT_N_IGN_TO_M( 4, 0);
regs[2] =
SHIFT_N_OUT_TO_M(22, 7) | /* TODO: stepper DIR */
SHIFT_N_OUT_TO_M(21, 6) | /* TODO: stepper enable */
SHIFT_N_OUT_TO_M(16, 5) |
SHIFT_N_OUT_TO_M(14, 4) |
SHIFT_N_OUT_TO_M(17, 3) |
SHIFT_N_OUT_TO_M(18, 2) |
SHIFT_N_OUT_TO_M( 7, 1) |
SHIFT_N_OUT_TO_M( 6, 0);
regs[3] =
SHIFT_N_OUT_TO_M(28, 5) |
SHIFT_N_OUT_TO_M(27, 4) |
SHIFT_N_OUT_TO_M(26, 3) |
SHIFT_N_OUT_TO_M(25, 2) |
SHIFT_N_OUT_TO_M(24, 1) |
SHIFT_N_OUT_TO_M(23, 0); /* TODO: stepper PWM */
uint16_t tx[] = {
/* output enables */
CMD_CONTR_REG(0, regs[0]),
CMD_CONTR_REG(1, regs[1]),
CMD_CONTR_REG(2, regs[2]),
CMD_CONTR_REG(3, regs[3])
};
ret = spi_rw_array(tx, NULL, ARRAY_SIZE(tx));
if (ret == 0) {
/* atomic */
o_data_cached = o_data;
}
return ret;
}
int L9779::update_direct_output(size_t pin, int value)
{
/* no direct-drive gpio is allocated for this output */
if (cfg->direct_gpio[pin].port == NULL)
return -1;
if (value)
palSetPort(cfg->direct_gpio[pin].port,
PAL_PORT_BIT(cfg->direct_gpio[pin].pad));
else
palClearPort(cfg->direct_gpio[pin].port,
PAL_PORT_BIT(cfg->direct_gpio[pin].pad));
return 0;
}
/**
* @brief L9779 chip driver wakeup.
* @details Wake up driver. Will cause output register update
*/
int L9779::wake_driver()
{
/* Entering a reentrant critical zone.*/
chibios_rt::CriticalSectionLocker csl;
chSemSignalI(&wake);
if (!port_is_isr_context()) {
/**
* chSemSignalI above requires rescheduling
* interrupt handlers have implicit rescheduling
*/
chSchRescheduleS();
}
return 0;
}
int L9779::chip_reset() {
int ret;
ret = spi_rw(CMD_CLOCK_UNLOCK_SW_RST(BIT(1)), NULL);
/**
* ???
*/
chThdSleepMilliseconds(3);
last_reg = REG_INVALID;
return ret;
}
/*==========================================================================*/
/* Driver thread. */
/*==========================================================================*/
static THD_FUNCTION(l9779_driver_thread, p) {
L9779 *chip = reinterpret_cast<L9779*>(p);
sysinterval_t poll_interval = 0;
chRegSetThreadName(DRIVER_NAME);
while (1) {
int ret;
msg_t msg = chSemWaitTimeout(&chip->wake, poll_interval);
/* should we care about msg == MSG_TIMEOUT? */
(void)msg;
/* default polling interval */
poll_interval = TIME_MS2I(DIAG_PERIOD_MS);
if ((chip->cfg == NULL) ||
(chip->drv_state == L9779_DISABLED) ||
(chip->drv_state == L9779_FAILED))
continue;
#if 0
bool wd_happy = chip->wd_happy;
/* update outputs only if WD is happy */
if ((wd_happy) || (1)) {
ret = chip->update_output();
if (ret) {
/* set state to L9779_FAILED? */
}
}
ret = chip->wd_feed();
if (ret < 0) {
/* WD is not happy */
continue;
}
/* happiness state has changed! */
if ((chip->wd_happy != wd_happy) && (chip->wd_happy)) {
chip->need_init = true;
}
#endif
if (chip->need_init) {
/* clear first, as flag can be raised again during init */
chip->need_init = false;
/* re-init chip! */
chip->chip_init();
/* sync pins state */
chip->update_output();
}
/* Chip is ready to rock? */
if (chip->need_init == false) {
/* Just update outputs state */
ret = chip->update_output();
if (ret) {
/* set state to L9779_FAILED? */
}
}
#if 0
if (chip->diag_ts <= chVTGetSystemTimeX()) {
/* this is expensive call, will do a lot of spi transfers... */
ret = chip->update_status_and_diag();
if (ret) {
/* set state to L9779_FAILED or force reinit? */
} else {
diagResponse.reset();
}
/* TODO:
* Procedure to switch on after failure condition occurred:
* - Read out of diagnosis bits
* - Second read out to verify that the failure conditions are not
* remaining
* - Set of the dedicated output enable bit of the affected channel
* if the diagnosis bit is not active anymore
* - Switch on of the channel */
chip->diag_ts = chTimeAddX(chVTGetSystemTimeX(), TIME_MS2I(DIAG_PERIOD_MS));
}
poll_interval = chip->calc_sleep_interval();
#endif
/* default poll_interval */
}
}
/*==========================================================================*/
/* Driver interrupt handlers. */
@ -213,7 +546,22 @@ int L9779::writePad(unsigned int pin, int value) {
if (pin >= L9779_OUTPUTS)
return -1;
(void)value;
{
chibios_rt::CriticalSectionLocker csl;
if (value) {
o_state |= (1 << pin);
} else {
o_state &= ~(1 << pin);
}
}
/* direct driven? */
if (OUT_DIRECT_DRIVE_MASK & BIT(pin)) {
return update_direct_output(pin, value);
} else {
return wake_driver();
}
return 0;
}
@ -242,7 +590,7 @@ int L9779::readPad(size_t pin) {
brain_pin_diag_e L9779::getDiag(size_t pin)
{
if (pin >= TLE8888_SIGNALS)
if (pin >= L9779_SIGNALS)
return PIN_INVALID;
if (pin < L9779_OUTPUTS)
@ -251,8 +599,99 @@ brain_pin_diag_e L9779::getDiag(size_t pin)
return getInputDiag(pin);
}
int L9779::chip_init_data(void)
{
int i;
int ret = 0;
o_oe_mask = 0;
for (i = 0; i < L9779_DIRECT_OUTPUTS; i++) {
if (cfg->direct_gpio[i].port == NULL)
continue;
/* configure source gpio */
ret = gpio_pin_markUsed(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad, DRIVER_NAME " DIRECT IO");
if (ret) {
ret = -1;
goto err_gpios;
}
palSetPadMode(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad, PAL_MODE_OUTPUT_PUSHPULL);
palClearPort(cfg->direct_gpio[i].port, PAL_PORT_BIT(cfg->direct_gpio[i].pad));
/* enable output */
o_oe_mask |= BIT(i);
}
/* enable all spi-driven ouputs
* TODO: add API to enable/disable? */
o_oe_mask |= ~OUT_DIRECT_DRIVE_MASK;
return 0;
err_gpios:
/* unmark pins */
for (int i = 0; i < L9779_DIRECT_OUTPUTS; i++) {
if (cfg->direct_gpio[i].port) {
gpio_pin_markUnused(cfg->direct_gpio[i].port, cfg->direct_gpio[i].pad);
}
}
return ret;
}
int L9779::chip_init()
{
int ret;
/* statistic */
init_cnt++;
/* Unlock, while unlocked by default. */
ret = spi_rw(CMD_CLOCK_UNLOCK_SW_RST(0), NULL);
if (ret)
return ret;
/* Enable power stages */
ret = spi_rw(CMD_START_REACT(BIT(1)), NULL);
if (ret)
return ret;
/* TODO: add spi communication test: read IDENT_REG */
return ret;
}
int L9779::init()
{
int ret;
/* check for multiple init */
if (drv_state != L9779_WAIT_INIT)
return -1;
ret = chip_reset();
if (ret)
return ret;
ret = chip_init_data();
if (ret)
return ret;
/* force chip init from driver thread */
need_init = true;
/* instance is ready */
drv_state = L9779_READY;
/* init semaphore */
chSemObjectInit(&wake, 10);
/* start thread */
thread = chThdCreateStatic(thread_wa, sizeof(thread_wa),
PRIO_GPIOCHIP, l9779_driver_thread, this);
return 0;
}

View File

@ -13,17 +13,29 @@
#include <hal.h>
#include "efifeatures.h"
#define L9779_OUTPUTS_IGN (4)
/* 4 x IGNI, IN1..IN6, PWM (IN7) */
#define L9779_DIRECT_OUTPUTS (L9779_OUTPUTS_IGN + 7)
#define L9779_OUTPUTS (L9779_OUTPUTS_IGN + 28 + 1)
#define L9779_INPUTS (1)
#define L9779_SIGNALS (L9779_OUTPUTS + L9779_INPUTS)
struct l9779_config {
#if HAL_USE_SPI
SPIDriver *spi_bus;
SPIConfig spi_config;
SPIDriver *spi_bus;
SPIConfig spi_config;
#endif
/* MCU port-pin routed to IGN1..IGN4, IN1..7 */
struct {
ioportid_t port;
uint_fast8_t pad;
} direct_gpio[L9779_DIRECT_OUTPUTS];
/* PWM(IN8) */
struct {
ioportid_t port;
uint_fast8_t pad;
} pwm_gpio;
};
#define L9779_OUTPUTS_IGN (4)
#define L9779_OUTPUTS (L9779_OUTPUTS_IGN + 28 + 1)
#define L9779_INPUTS (1)
#define L9779_SIGNALS (L9779_OUTPUTS + L9779_INPUTS)
int l9779_add(brain_pin_e base, unsigned int index, const l9779_config *cfg);

View File

@ -116,6 +116,23 @@ struct l9779_config l9779_cfg = {
0,
.cr2 = SPI_CR2_16BIT_MODE
},
.direct_gpio = {
/* IGNI1..IGNI4 */
[0] = {.port = NULL, .pad = 0},
[1] = {.port = NULL, .pad = 0},
[2] = {.port = NULL, .pad = 0},
[3] = {.port = NULL, .pad = 0},
/* IN1..IN7 */
[4] = {.port = NULL, .pad = 0},
[5] = {.port = NULL, .pad = 0},
[6] = {.port = NULL, .pad = 0},
[7] = {.port = NULL, .pad = 0},
[8] = {.port = NULL, .pad = 0},
[9] = {.port = NULL, .pad = 0},
[10] = {.port = NULL, .pad = 0},
},
/* PWM (IN8) */
.pwm_gpio = {.port = NULL, .pad = 0},
};
#endif /* (BOARD_L9779_COUNT > 0) */