Aksal/nuttx
3
1
Fork 1
mirror of https://github.com/apache/nuttx.git synced 2026-08-01 20:28:58 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 2
nuttx/drivers/sensors/vl53l1x.c
Catalin Visinescu 081e4c478a drivers/: Multiple Drivers Are Registered With World Writable - Part 2 
Permissions (Part 2)

Description:

In kernel builds, any unprivileged process running on the NuttX
device can open /dev/efuse and attempt to read/write fuse content.
Reading the fuses may provide valuable information to an attacker
controlling the user process. The write operation, in extreme cases
where the fuse blocks are not locked, may brick the device.

DISCLAIMER: I tried to be strict with the settings, better to relax them
later if it's needed.

This is part of https://github.com/apache/nuttx/issues/19410

See https://github.com/apache/nuttx/issues/19410

Compiles ok.

Signed-off-by: Catalin Visinescu <catalin_visinescu@yahoo.com>
2026-07-15 15:27:28 +08:00

1171 lines
30 KiB
C
Raw Permalink Blame History

/****************************************************************************
* drivers/sensors/vl53l1x.c
*
* SPDX-License-Identifier: BSD-3-Clause
* SPDX-FileCopyrightText: Copyright (C) 2019 Acutronics Robotics
* SPDX-FileCopyrightText: Acutronics Robotics
* SPDX-FileCopyrightText: (Juan Flores Muñoz) <juan@erlerobotics.com>
*
* 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 NuttX 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 OWNER 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.
*
****************************************************************************/
/* WARNING for developers:
*
* This driver uses the legacy style of writing sensor drivers for NuttX. The
* project has since decided to adopt a new sensor framework in order to
* have a consistent API and feature-set.
*
* Sensors which use the uORB framework are typically suffixed "_uorb". You
* can also visit the documentation about the new sensor framework to learn
* more.
*/
#warning "This is a deprecated legacy sensor driver."
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdlib.h>
#include <fixedmath.h>
#include <errno.h>
#include <nuttx/debug.h>
#include <nuttx/kmalloc.h>
#include <nuttx/signal.h>
#include <nuttx/fs/fs.h>
#include <nuttx/i2c/i2c_master.h>
#include <nuttx/sensors/vl53l1x.h>
#include <nuttx/sensors/ioctl.h>
#include <nuttx/random.h>
#if defined(CONFIG_I2C) && defined(CONFIG_SENSORS_VL53L1X)
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define VL53L1X_FREQ 100000
#define VL53L1X_ADDR 0x29
/****************************************************************************
* Private Types
****************************************************************************/
struct vl53l1x_dev_s
{
FAR struct i2c_master_s *i2c; /* I2C interface */
uint8_t addr; /* VL53L0X I2C address */
int freq; /* VL53L0X Frequency */
};
/****************************************************************************
* Private Data
****************************************************************************/
static const uint8_t g_vl51l1x_default_config[] =
{
0x00,
0x00,
0x00,
0x01,
0x02,
0x00,
0x02,
0x08,
0x00,
0x08,
0x10,
0x01,
0x01,
0x00,
0x00,
0x00,
0x00,
0xff,
0x00,
0x0f,
0x00,
0x00,
0x00,
0x00,
0x00,
0x20,
0x0b,
0x00,
0x00,
0x02,
0x0a,
0x21,
0x00,
0x00,
0x05,
0x00,
0x00,
0x00,
0x00,
0xc8,
0x00,
0x00,
0x38,
0xff,
0x01,
0x00,
0x08,
0x00,
0x00,
0x01,
0xdb,
0x0f,
0x01,
0xf1,
0x0d,
0x01,
0x68,
0x00,
0x80,
0x08,
0xb8,
0x00,
0x00,
0x00,
0x00,
0x0f,
0x89,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
0x00,
0x01,
0x0f,
0x0d,
0x0e,
0x0e,
0x00,
0x00,
0x02,
0xc7,
0xff,
0x9b,
0x00,
0x00,
0x00,
0x01,
0x00,
0x00
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
/* I2C operations */
static uint8_t vl53l1x_getreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static uint16_t vl53l1x_getreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static uint32_t vl53l1x_getreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr);
static void vl53l1x_putreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint8_t regval);
static void vl53l1x_putreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint16_t regval);
static void vl53l1x_putreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint32_t regval);
/* Basic driver operations */
static void vl53l1x_startranging(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_stopranging(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_sensorinit(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_getdistance(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *distance);
/* Set data operations */
static void vl53l1x_settimingbudget(FAR struct vl53l1x_dev_s *priv,
uint16_t ms_value);
static void vl53l1x_setdistancemode(FAR struct vl53l1x_dev_s *priv,
uint16_t mode);
/* Get data operations */
static void vl53l1x_gettimingbudget(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *ms_value);
static void vl53l1x_getdistancemode(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *dm);
static void vl53l1x_getid(FAR struct vl53l1x_dev_s *priv, FAR uint16_t *id);
static void vl53l1x_getoffset(FAR struct vl53l1x_dev_s *priv,
FAR int16_t *offset);
static void vl53l1x_getintstate(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *state);
/* Other operations */
static void vl53l1x_clearint(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_dataready(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *dataready);
static void vl53l1x_tempupdate(FAR struct vl53l1x_dev_s *priv);
static void vl53l1x_calibrateoffset(FAR struct vl53l1x_dev_s *priv,
uint16_t target_distance,
FAR int16_t *offset);
/* Character driver methods */
static void vl53l1x_read(FAR struct file *filep, FAR char *buffer,
size_t buflen);
static ssize_t vl53l1x_write(FAR struct file *filep, FAR const char *buffer,
size_t buflen);
static void vl53l1x_ioctl(FAR struct file *filep, int cmd, uint16_t arg);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct file_operations g_vl53l1xfops =
{
NULL, /* open */
NULL, /* close */
vl53l1x_read, /* read */
vl53l1x_write, /* write */
NULL, /* seek */
vl53l1x_ioctl, /* ioctl */
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: vl53l1x_SensorInit
*
* Description:
* This function loads the 135 bytes default values to initialize the
* sensor.
*
****************************************************************************/
static void vl53l1x_sensorinit(FAR struct vl53l1x_dev_s *priv)
{
uint8_t addr = 0x00;
uint8_t tmp = 0;
for (addr = 0x2d; addr <= 0x87; addr++)
{
vl53l1x_putreg8(priv, addr, g_vl51l1x_default_config[addr - 0x2d]);
}
vl53l1x_startranging(priv);
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_clearint(priv);
vl53l1x_stopranging(priv);
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x09);
vl53l1x_putreg8(priv, 0x0b, 0);
}
/****************************************************************************
* Name: vl53l1x_clearint
*
* Description:
* This function clears the interrupt, to be called after a ranging data
* reading to arm the interrupt for the next data ready event.
*
****************************************************************************/
static void vl53l1x_clearint(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, INTERRUPT_CLEAR, 0x01);
}
/****************************************************************************
* Name: vl53l1x_startranging
*
* Description:
* This function starts the ranging distance operation. The ranging ops
* is continuous. The clear interrupt has to be done after each get data to
* allow the interrupt to raise when the next data is ready.
*
****************************************************************************/
static void vl53l1x_startranging(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, SYSTEM_MODE, 0x40);
}
/****************************************************************************
* Name: vl53l1x_StopRanging
*
* Description:
* This function stops the ranging.
*
****************************************************************************/
static void vl53l1x_stopranging(FAR struct vl53l1x_dev_s *priv)
{
vl53l1x_putreg8(priv, SYSTEM_MODE, 0x00);
}
/****************************************************************************
* Name: vl53l1x_dataready
*
* Description:
* This function checks if the new ranging data is available by polling the
* dedicated register.
*
****************************************************************************/
static void vl53l1x_dataready(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *dataready)
{
uint8_t temp;
uint8_t intpol;
temp = vl53l1x_getreg8(priv, GPIO_STATUS);
vl53l1x_getintstate(priv, &intpol);
/* Read in the register to check if a new value is available */
if ((temp & 1) == intpol)
{
*dataready = 1;
}
else
{
*dataready = 0;
}
}
/****************************************************************************
* Name: vl53l1x_getinterruptstate
*
* Description:
* This function returns the current interrupt polarity.
*
****************************************************************************/
static void vl53l1x_getintstate(FAR struct vl53l1x_dev_s *priv,
FAR uint8_t *state)
{
uint8_t temp;
temp = vl53l1x_getreg8(priv, GPIO_MUX_CTRL);
temp = temp & 0x10;
*state = !(temp >> 4);
}
/****************************************************************************
* Name: VL53L1X_GetTimingBudgetInMs
*
* Description:
* This function returns the timing budget in ms.
*
****************************************************************************/
static void vl53l1x_gettimingbudget(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *ms_value)
{
uint16_t temp;
temp = vl53l1x_getreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI);
switch (temp)
{
case 0x001d:
*ms_value = 15;
break;
case 0x0051:
case 0x001e:
*ms_value = 20;
break;
case 0x00d6:
case 0x0060:
*ms_value = 33;
break;
case 0x1ae:
case 0x00ad:
*ms_value = 50;
break;
case 0x02e1:
case 0x01cc:
*ms_value = 100;
break;
case 0x03e1:
case 0x02d9:
*ms_value = 200;
break;
case 0x0591:
case 0x048f:
*ms_value = 500;
break;
default:
*ms_value = 0;
break;
}
}
/****************************************************************************
* Name: vl53l1x_settimingbudget
*
* Description:
* This function programs the timing budget in ms.
*
****************************************************************************/
static void vl53l1x_settimingbudget(FAR struct vl53l1x_dev_s *priv,
uint16_t ms_value)
{
uint16_t dm;
vl53l1x_getdistancemode(priv, &dm);
/* Short Distance */
if (dm == 1)
{
switch (ms_value)
{
case 15:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x01d);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0027);
}
break;
case 20:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0051);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 33:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x00d6);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 50:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x1ae);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x01e8);
}
break;
case 100:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x02e1);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0388);
}
break;
case 200:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x03e1);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0496);
}
break;
case 500:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0591);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x05c1);
}
break;
default:
break;
}
}
else
{
switch (ms_value)
{
case 20:
{
vl53l1x_putreg16(priv, PHASECAL_TIMEOUT_MACRO, 0x001e);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x0022);
}
break;
case 33:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x0060);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x006e);
}
break;
case 50:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x00ad);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x00c6);
}
break;
case 100:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x01cc);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x01ea);
}
break;
case 200:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x02d9);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x02f8);
}
break;
case 500:
{
vl53l1x_putreg16(priv, RANGE_CFG_TIMEOUT_MACRO_HI, 0x048f);
vl53l1x_putreg16(priv, RANGE_TIMEOUT_MACRO_HI, 0x04a4);
}
break;
default:
break;
}
}
}
/****************************************************************************
* Name: vl53l1x_setdistancemode
*
* Description:
* This function programs the distance mode (1=short, 2=long(default)).
* Short mode max distance is limited to 1.3 m but better ambient immunity.
* Long mode can range up to 4 m in the dark with 200 ms timing budget.
*
****************************************************************************/
static void vl53l1x_setdistancemode(FAR struct vl53l1x_dev_s *priv,
uint16_t dm)
{
uint16_t tb;
vl53l1x_gettimingbudget(priv, &tb);
switch (dm)
{
case 1:
{
vl53l1x_putreg8(priv, PHASECAL_TIMEOUT_MACRO, 0x14);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_A, 0x07);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_B, 0x05);
vl53l1x_putreg8(priv, RANGE_CFG_VALID_PHASE, 0x38);
vl53l1x_putreg16(priv, SD_CFG_WOI_SD0, 0x0705);
vl53l1x_putreg16(priv, SD_CFG_INIT_PHASE, 0x0606);
}
break;
case 2:
{
vl53l1x_putreg8(priv, PHASECAL_TIMEOUT_MACRO, 0x0a);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_A, 0x0f);
vl53l1x_putreg8(priv, RANGE_VCSEL_PERIOD_B, 0x0d);
vl53l1x_putreg8(priv, RANGE_CFG_VALID_PHASE, 0xb8);
vl53l1x_putreg16(priv, SD_CFG_WOI_SD0, 0x0f0d);
vl53l1x_putreg16(priv, SD_CFG_INIT_PHASE, 0x0e0e);
}
break;
default:
break;
}
vl53l1x_settimingbudget(priv, tb);
}
/****************************************************************************
* Name: vl53l1x_getdistancemode
*
* Description:
* This function returns the current distance mode (1=short, 2=long).
*
****************************************************************************/
static void vl53l1x_getdistancemode(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *dm)
{
uint8_t mode_read;
mode_read = vl53l1x_getreg8(priv, PHASECAL_TIMEOUT_MACRO);
if (mode_read == 0x14)
{
*dm = 1;
}
if (mode_read == 0x0a)
{
*dm = 2;
}
}
/****************************************************************************
* Name: vl53l1x_getid
*
* Description:
* This function returns the sensor id, sensor Id must be 0xeeac.
*
****************************************************************************/
static void vl53l1x_getid(FAR struct vl53l1x_dev_s *priv, FAR uint16_t *id)
{
uint16_t tmp = 0;
tmp = vl53l1x_getreg16(priv, VL53L1_GET_ID);
*id = tmp;
}
/****************************************************************************
* Name: vl53l1x_GetDistance
*
* Description:
* This function returns the distance measured by the sensor in mm.
*
****************************************************************************/
static void vl53l1x_getdistance(FAR struct vl53l1x_dev_s *priv,
FAR uint16_t *distance)
{
uint16_t tmp;
tmp = vl53l1x_getreg16(priv, VL53L1_GET_DISTANCE);
*distance = tmp;
}
/****************************************************************************
* Name: vl53l1x_getoffset
*
* Description:
* This function returns the programmed offset correction value in mm.
*
****************************************************************************/
static void vl53l1x_getoffset(FAR struct vl53l1x_dev_s *priv,
FAR int16_t *offset)
{
uint16_t temp;
temp = vl53l1x_getreg16(priv, RANGE_OFFSET_MM);
temp = temp << 3;
temp = temp >> 5;
*offset = (int16_t) (temp);
}
/****************************************************************************
* Name: vl53l1x_tempupdate
*
* Description:
* This function performs the temperature calibration. It is recommended to
* call this function any time the temperature might have changed by more
* than 8 deg C without sensor ranging activity for an extended period.
*
****************************************************************************/
static void vl53l1x_tempupdate(FAR struct vl53l1x_dev_s *priv)
{
uint8_t tmp = 0;
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x81);
vl53l1x_putreg8(priv, 0x0b, 0x92);
vl53l1x_startranging(priv);
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_clearint(priv);
vl53l1x_stopranging(priv);
vl53l1x_putreg8(priv, TIMEOUT_MACROP_LOOP_BOUND, 0x09);
vl53l1x_putreg8(priv, 0x0b, 0);
}
/****************************************************************************
* Name: vl53l1x_calibrateoffset
*
* Description:
* The function returns the offset value found and programs the offset
* compensation into the device.
*
****************************************************************************/
static void vl53l1x_calibrateoffset(FAR struct vl53l1x_dev_s *priv,
uint16_t target_distance,
FAR int16_t *offset)
{
uint8_t i = 0;
uint8_t tmp;
int16_t average_distance = 0;
uint16_t distance;
vl53l1x_putreg16(priv, RANGE_OFFSET_MM, 0x0);
vl53l1x_putreg16(priv, INNER_OFFSET_MM, 0x0);
vl53l1x_putreg16(priv, OUTER_OFFSET_MM, 0x0);
vl53l1x_startranging(priv);
for (i = 0; i < 50; i++)
{
while (tmp == 0)
{
vl53l1x_dataready(priv, &tmp);
}
vl53l1x_getdistance(priv, &distance);
vl53l1x_clearint(priv);
average_distance = average_distance + distance;
}
vl53l1x_stopranging(priv);
average_distance = average_distance / 50;
*offset = target_distance - average_distance;
vl53l1x_putreg16(priv, RANGE_OFFSET_MM, *offset *4);
}
/****************************************************************************
* Name: vl53l1x_getreg8
*
* Description:
* Read from an 8-bit vl53l1x register
*
****************************************************************************/
static uint8_t vl53l1x_getreg8(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint8_t regval = 0;
uint16_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Write the register address */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&regaddr, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read the register value */
ret = i2c_read(priv->i2c, &config, &regval, 1);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
return regval;
}
/****************************************************************************
* Name: bmp180_getreg16
*
* Description:
* Read two 8-bit from a BMP180 register
*
****************************************************************************/
static uint16_t vl53l1x_getreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint16_t msb;
uint16_t lsb;
uint16_t regval = 0;
uint8_t reg_addr_aux[2];
uint8_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Split the I2C direction */
reg_addr_aux[0] = (regaddr >> 8) & 0xff;
reg_addr_aux[1] = regaddr;
/* Register to read */
sninfo("Reg %02x %\n", reg_addr_aux[0], reg_addr_aux[1]);
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&reg_addr_aux, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read register */
ret = i2c_read(priv->i2c, &config, (FAR uint8_t *) & regval, 2);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
/* MSB and LSB are inverted */
msb = (regval & 0xff);
lsb = (regval & 0xff00) >> 8;
regval = (msb << 8) | lsb;
return regval;
}
/****************************************************************************
* Name: vl53l1x_getreg32
*
* Description:
* Read 4 8-bit from a vl53l1x register
*
****************************************************************************/
static uint32_t vl53l1x_getreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr)
{
struct i2c_config_s config;
uint16_t byte[4];
uint32_t regval = 0;
uint8_t ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
/* Register to read */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *)&regaddr, 2);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return ret;
}
/* Read register */
ret = i2c_read(priv->i2c, &config, (FAR uint8_t *) & regval, 4);
if (ret < 0)
{
snerr("ERROR: i2c_read failed: %d\n", ret);
return ret;
}
/* The bytes are in the opposite order of importance */
byte[0] = (regval & 0xff);
byte[1] = (regval & 0xff00) >> 8;
byte[2] = (regval & 0xffff00) >> 16;
byte[3] = (regval & 0xffffff00) >> 24;
regval = (byte[0] << 24) | (byte[1] << 16) | (byte[2] << 8) | byte[3];
return regval;
}
/****************************************************************************
* Name: vl53l1x_putreg8
*
* Description:
* Write to an 8-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg8(FAR struct vl53l1x_dev_s *priv, uint16_t regaddr,
uint8_t regval)
{
struct i2c_config_s config;
uint8_t data[3];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 3);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_putreg16
*
* Description:
* Write to an 16-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg16(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint16_t regval)
{
struct i2c_config_s config;
uint8_t data[4];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = (regval >> 8) & 0xff;
data[3] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 4);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_putreg32
*
* Description:
* Write to an 32-bit vl53l1x register
*
****************************************************************************/
static void vl53l1x_putreg32(FAR struct vl53l1x_dev_s *priv,
uint16_t regaddr, uint32_t regval)
{
struct i2c_config_s config;
uint8_t data[7];
int ret;
/* Set up the I2C configuration */
config.frequency = priv->freq;
config.address = priv->addr;
config.addrlen = 7;
data[0] = (regaddr >> 8) & 0xff;
data[1] = regaddr;
data[2] = (regval >> 24) & 0xff;
data[4] = (regval >> 16) & 0xff;
data[5] = (regval >> 8) & 0xff;
data[6] = regval & 0xff;
/* Write the register address and value */
ret = i2c_write(priv->i2c, &config, (FAR uint8_t *) & data, 7);
if (ret < 0)
{
snerr("ERROR: i2c_write failed: %d\n", ret);
return;
}
}
/****************************************************************************
* Name: vl53l1x_read
****************************************************************************/
static void vl53l1x_read(FAR struct file *filep, FAR char *buffer,
size_t buflen)
{
FAR struct inode *inode = filep->f_inode;
FAR struct vl53l1x_dev_s *priv = inode->i_private;
FAR uint16_t *aux = (FAR uint16_t *) buffer;
vl53l1x_startranging(priv);
vl53l1x_getdistance(priv, aux);
vl53l1x_stopranging(priv);
}
/****************************************************************************
* Name: vl53l1x_write
****************************************************************************/
static ssize_t vl53l1x_write(FAR struct file *filep, FAR const char *buffer,
size_t buflen)
{
return -ENOSYS;
}
/****************************************************************************
* Name: vl53l1x_ioctl
****************************************************************************/
static void vl53l1x_ioctl(FAR struct file *filep, int cmd, uint16_t arg)
{
FAR struct inode *inode = filep->f_inode;
FAR struct vl53l1x_dev_s *priv = inode->i_private;
switch (cmd)
{
case SNIOC_DISTANCESHORT:
{
sninfo("Set distance up to 1.3M\n");
vl53l1x_setdistancemode(priv, 1);
}
break;
case SNIOC_DISTANCELONG:
{
sninfo("Set distance up to 4M\n");
vl53l1x_setdistancemode(priv, 2);
}
break;
case SNIOC_CALIBRATE:
{
sninfo("Calibrating distance\n");
int16_t offset;
vl53l1x_getoffset(priv, (FAR int16_t *)&offset);
vl53l1x_calibrateoffset(priv, arg, (FAR int16_t *)&offset);
}
break;
case SNIOC_TEMPUPDATE:
{
sninfo("Recalculating due to temperature change\n");
vl53l1x_tempupdate(priv);
}
break;
}
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: vl53l1x_register
*
* Description:
* Register the vl53l1x character device as 'devpath'
*
* Input Parameters:
* devpath - The full path to the driver to register. E.g., "/dev/tof"
* i2c - An instance of the I2C interface to use to communicate with
* vl53l1x TOF
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int vl53l1x_register(FAR const char *devpath, FAR struct i2c_master_s *i2c)
{
FAR struct vl53l1x_dev_s *priv;
int ret = 0;
uint16_t id;
/* Initialize the vl53l1x device structure */
priv = kmm_malloc(sizeof(struct vl53l1x_dev_s));
if (!priv)
{
snerr("ERROR: Failed to allocate instance\n");
return -ENOMEM;
}
priv->i2c = i2c;
priv->addr = VL53L1X_ADDR;
priv->freq = VL53L1X_FREQ;
vl53l1x_sensorinit(priv);
vl53l1x_getid(priv, &id);
if (id != 0xeacc)
{
snerr("ERROR: Failed sensor ID %04x\n", id);
kmm_free(priv);
return 0;
}
register_driver(devpath, &g_vl53l1xfops, 0600, priv);
if (ret < 0)
{
snerr("ERROR: Failed to register driver: %d\n", ret);
kmm_free(priv);
return 0;
}
sninfo("vl53l1x driver loaded successfully!\n");
return 1;
}
#endif /* CONFIG_I2C && CONFIG_SENSORS_VL53L1X */
Reference in a new issue View git blame Copy permalink