1 /* vl53l0x_platform.c - Zephyr customization of ST vl53l0x library.
2 * (library is located in ext/hal/st/lib/sensor/vl53l0x/)
3 */
4
5 /*
6 * Copyright (c) 2017 STMicroelectronics
7 *
8 * SPDX-License-Identifier: Apache-2.0
9 */
10
11 #include "vl53l0x_platform.h"
12
13 #include <zephyr/drivers/sensor.h>
14 #include <zephyr/kernel.h>
15 #include <zephyr/device.h>
16 #include <zephyr/init.h>
17 #include <zephyr/drivers/i2c.h>
18 #include <zephyr/logging/log.h>
19
20 LOG_MODULE_DECLARE(VL53L0X, CONFIG_SENSOR_LOG_LEVEL);
21
VL53L0X_WriteMulti(VL53L0X_DEV Dev,uint8_t index,uint8_t * pdata,uint32_t count)22 VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata,
23 uint32_t count)
24 {
25
26 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
27 int32_t status_int = 0;
28 uint8_t I2CBuffer[count+1];
29
30 I2CBuffer[0] = index;
31 memcpy(&I2CBuffer[1], pdata, count);
32
33 status_int = i2c_write(Dev->i2c, I2CBuffer, count+1, Dev->I2cDevAddr);
34
35 if (status_int < 0) {
36 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
37 LOG_ERR("Failed to write");
38 }
39
40 return Status;
41 }
42
VL53L0X_ReadMulti(VL53L0X_DEV Dev,uint8_t index,uint8_t * pdata,uint32_t count)43 VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata,
44 uint32_t count)
45 {
46 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
47 int32_t status_int;
48
49 status_int = i2c_burst_read(Dev->i2c, Dev->I2cDevAddr, index, pdata,
50 count);
51 if (status_int < 0) {
52 LOG_ERR("Failed to read");
53 return -EIO;
54 }
55
56 return Status;
57 }
58
59
VL53L0X_WrByte(VL53L0X_DEV Dev,uint8_t index,uint8_t data)60 VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
61 {
62 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
63 int32_t status_int;
64
65 status_int = i2c_reg_write_byte(Dev->i2c, Dev->I2cDevAddr, index, data);
66
67 if (status_int < 0) {
68 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
69 LOG_ERR("i2c_reg_write_byte failed (%d)", Status);
70 }
71
72 return Status;
73 }
74
VL53L0X_WrWord(VL53L0X_DEV Dev,uint8_t index,uint16_t data)75 VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
76 {
77 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
78 int32_t status_int;
79 uint8_t I2CBuffer[3];
80
81 I2CBuffer[0] = index;
82 I2CBuffer[1] = data >> 8;
83 I2CBuffer[2] = data & 0x00FF;
84
85 status_int = i2c_write(Dev->i2c, I2CBuffer, 3, Dev->I2cDevAddr);
86 if (status_int < 0) {
87 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
88 LOG_ERR("i2c_write failed (%d)", Status);
89 }
90
91 return Status;
92 }
93
VL53L0X_WrDWord(VL53L0X_DEV Dev,uint8_t index,uint32_t data)94 VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
95 {
96 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
97 int32_t status_int;
98 uint8_t I2CBuffer[5];
99
100 I2CBuffer[0] = index;
101 I2CBuffer[1] = (data >> 24) & 0xFF;
102 I2CBuffer[2] = (data >> 16) & 0xFF;
103 I2CBuffer[3] = (data >> 8) & 0xFF;
104 I2CBuffer[4] = (data >> 0) & 0xFF;
105
106 status_int = i2c_write(Dev->i2c, I2CBuffer, 5, Dev->I2cDevAddr);
107 if (status_int < 0) {
108 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
109 LOG_ERR("i2c_write failed (%d)", Status);
110 }
111
112 return Status;
113 }
114
VL53L0X_UpdateByte(VL53L0X_DEV Dev,uint8_t index,uint8_t AndData,uint8_t OrData)115 VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index,
116 uint8_t AndData, uint8_t OrData)
117 {
118 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
119 int32_t status_int;
120 uint8_t deviceAddress;
121 uint8_t data;
122
123 deviceAddress = Dev->I2cDevAddr;
124
125 status_int = VL53L0X_RdByte(Dev, index, &data);
126 if (status_int < 0) {
127 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
128 LOG_ERR("VL53L0X_RdByte failed (%d)", Status);
129 }
130
131 if (Status == VL53L0X_ERROR_NONE) {
132 data = (data & AndData) | OrData;
133 status_int = VL53L0X_WrByte(Dev, index, data);
134 if (status_int != 0) {
135 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
136 LOG_DBG("VL53L0X_WrByte failed.(%d)", Status);
137 }
138 }
139
140 return Status;
141 }
142
VL53L0X_RdByte(VL53L0X_DEV Dev,uint8_t index,uint8_t * data)143 VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
144 {
145 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
146 int32_t status_int;
147
148 status_int = i2c_reg_read_byte(Dev->i2c, Dev->I2cDevAddr, index, data);
149 if (status_int < 0) {
150 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
151 LOG_ERR("i2c_reg_read_byte failed (%d)", Status);
152 }
153
154 return Status;
155 }
156
VL53L0X_RdWord(VL53L0X_DEV Dev,uint8_t index,uint16_t * data)157 VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
158 {
159 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
160 int32_t status_int;
161 uint8_t buf[2];
162
163 status_int = i2c_burst_read(Dev->i2c, Dev->I2cDevAddr, index, buf, 2);
164 if (status_int < 0) {
165 LOG_ERR("i2c_burst_read failed");
166 return -EIO;
167 }
168 *data = ((uint16_t)buf[0]<<8) + (uint16_t)buf[1];
169
170 return Status;
171 }
172
VL53L0X_RdDWord(VL53L0X_DEV Dev,uint8_t index,uint32_t * data)173 VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
174 {
175 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
176 int32_t status_int;
177 uint8_t buf[4];
178
179 status_int = i2c_burst_read(Dev->i2c, Dev->I2cDevAddr, index, buf, 4);
180 if (status_int < 0) {
181 LOG_ERR("i2c_burst_read failed");
182 return -EIO;
183 }
184 *data = ((uint32_t)buf[0]<<24) + ((uint32_t)buf[1]<<16) +
185 ((uint32_t)buf[2]<<8) + (uint32_t)buf[3];
186
187 return Status;
188 }
189
VL53L0X_PollingDelay(VL53L0X_DEV Dev)190 VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
191 {
192 k_sleep(K_MSEC(2));
193 return VL53L0X_ERROR_NONE;
194 }
195
