1 /* vl53l1_platform.c - Zephyr customization of ST vl53l1x library.
2 */
3
4 /*
5 * Copyright (c) 2023 Prosaris Solutions Inc.
6 *
7 * SPDX-License-Identifier: Apache-2.0
8 */
9
10 #include "vl53l1_platform.h"
11 #include "vl53l1_platform_log.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 #include <zephyr/sys/byteorder.h>
20
21 LOG_MODULE_DECLARE(VL53L1X, CONFIG_SENSOR_LOG_LEVEL);
22
VL53L1_WriteMulti(VL53L1_Dev_t * pdev,uint16_t reg,uint8_t * pdata,uint32_t count)23 VL53L1_Error VL53L1_WriteMulti(VL53L1_Dev_t *pdev, uint16_t reg,
24 uint8_t *pdata, uint32_t count)
25 {
26 VL53L1_Error status = VL53L1_ERROR_NONE;
27 int32_t status_int = 0;
28 uint8_t buffer[count + 2];
29
30 /* To be able to write to the 16-bit registers/addresses on the vl53l1x */
31 buffer[1] = (uint8_t)(reg & 0x00ff);
32 buffer[0] = (uint8_t)((reg & 0xff00) >> 8);
33
34 memcpy(&buffer[2], pdata, count);
35
36 status_int = i2c_write_dt(pdev->i2c, buffer, count + 2);
37
38 if (status_int < 0) {
39 status = VL53L1_ERROR_CONTROL_INTERFACE;
40 LOG_ERR("Failed to write");
41 }
42
43 return status;
44 }
45
VL53L1_ReadMulti(VL53L1_Dev_t * pdev,uint16_t reg,uint8_t * pdata,uint32_t count)46 VL53L1_Error VL53L1_ReadMulti(VL53L1_Dev_t *pdev, uint16_t reg,
47 uint8_t *pdata, uint32_t count)
48 {
49 VL53L1_Error status = VL53L1_ERROR_NONE;
50 int32_t status_int = 0;
51
52 reg = sys_cpu_to_be16(reg);
53
54 status_int = i2c_write_read_dt(pdev->i2c, (uint8_t *)(®), 2, pdata, count);
55
56 if (status_int < 0) {
57 status = VL53L1_ERROR_CONTROL_INTERFACE;
58 LOG_ERR("Failed to read");
59 return -EIO;
60 }
61
62 return status;
63 }
64
VL53L1_WrByte(VL53L1_Dev_t * pdev,uint16_t reg,uint8_t data)65 VL53L1_Error VL53L1_WrByte(VL53L1_Dev_t *pdev, uint16_t reg, uint8_t data)
66 {
67 VL53L1_Error status = VL53L1_ERROR_NONE;
68
69 status = VL53L1_WriteMulti(pdev, reg, &data, 1);
70
71 return status;
72 }
73
VL53L1_WrWord(VL53L1_Dev_t * pdev,uint16_t reg,uint16_t data)74 VL53L1_Error VL53L1_WrWord(VL53L1_Dev_t *pdev, uint16_t reg, uint16_t data)
75 {
76 VL53L1_Error status = VL53L1_ERROR_NONE;
77
78 data = sys_cpu_to_be16(data);
79
80 status = VL53L1_WriteMulti(pdev, reg, (uint8_t *)(&data), VL53L1_BYTES_PER_WORD);
81
82 return status;
83 }
84
VL53L1_WrDWord(VL53L1_Dev_t * pdev,uint16_t reg,uint32_t data)85 VL53L1_Error VL53L1_WrDWord(VL53L1_Dev_t *pdev, uint16_t reg, uint32_t data)
86 {
87 VL53L1_Error status = VL53L1_ERROR_NONE;
88
89 data = sys_cpu_to_be32(data);
90
91 status = VL53L1_WriteMulti(pdev, reg, (uint8_t *)(&data), VL53L1_BYTES_PER_DWORD);
92
93 return status;
94 }
95
VL53L1_RdByte(VL53L1_Dev_t * pdev,uint16_t reg,uint8_t * pdata)96 VL53L1_Error VL53L1_RdByte(VL53L1_Dev_t *pdev, uint16_t reg, uint8_t *pdata)
97 {
98 VL53L1_Error status = VL53L1_ERROR_NONE;
99
100 status = VL53L1_ReadMulti(pdev, reg, pdata, 1);
101
102 return status;
103 }
104
VL53L1_RdWord(VL53L1_Dev_t * pdev,uint16_t reg,uint16_t * pdata)105 VL53L1_Error VL53L1_RdWord(VL53L1_Dev_t *pdev, uint16_t reg, uint16_t *pdata)
106 {
107 VL53L1_Error status = VL53L1_ERROR_NONE;
108
109 status = VL53L1_ReadMulti(pdev, reg, (uint8_t *)pdata, 2);
110 *pdata = sys_be16_to_cpu(*pdata);
111
112 return status;
113 }
114
VL53L1_RdDWord(VL53L1_Dev_t * pdev,uint16_t reg,uint32_t * pdata)115 VL53L1_Error VL53L1_RdDWord(VL53L1_Dev_t *pdev, uint16_t reg, uint32_t *pdata)
116 {
117 VL53L1_Error status = VL53L1_ERROR_NONE;
118
119 status = VL53L1_ReadMulti(pdev, reg, (uint8_t *)pdata, 4);
120 *pdata = sys_be32_to_cpu(*pdata);
121
122 return status;
123 }
124
VL53L1_WaitUs(VL53L1_Dev_t * pdev,int32_t wait_us)125 VL53L1_Error VL53L1_WaitUs(VL53L1_Dev_t *pdev, int32_t wait_us)
126 {
127 k_sleep(K_USEC(wait_us));
128 return VL53L1_ERROR_NONE;
129 }
130
VL53L1_WaitMs(VL53L1_Dev_t * pdev,int32_t wait_ms)131 VL53L1_Error VL53L1_WaitMs(VL53L1_Dev_t *pdev, int32_t wait_ms)
132 {
133 return VL53L1_WaitUs(pdev, wait_ms * 1000);
134 }
135
VL53L1_GetTickCount(uint32_t * ptick_count_ms)136 VL53L1_Error VL53L1_GetTickCount(uint32_t *ptick_count_ms)
137 {
138 *ptick_count_ms = k_uptime_get_32();
139 return VL53L1_ERROR_NONE;
140 }
141
VL53L1_WaitValueMaskEx(VL53L1_Dev_t * dev,uint32_t timeout,uint16_t i,uint8_t val,uint8_t mask,uint32_t delay)142 VL53L1_Error VL53L1_WaitValueMaskEx(VL53L1_Dev_t *dev, uint32_t timeout, uint16_t i, uint8_t val,
143 uint8_t mask, uint32_t delay)
144 {
145 VL53L1_Error status = VL53L1_ERROR_NONE;
146 uint32_t start_time_ms = 0;
147 uint32_t current_time_ms = 0;
148 uint8_t byte_val = 0;
149 uint8_t found = 0;
150
151 /* calculate time limit in absolute time */
152 VL53L1_GetTickCount(&start_time_ms);
153 dev->new_data_ready_poll_duration_ms = 0;
154
155 /* wait until val is found, timeout reached on error occurred */
156 while ((status == VL53L1_ERROR_NONE) &&
157 (dev->new_data_ready_poll_duration_ms < timeout) &&
158 (found == 0)) {
159 status = VL53L1_RdByte(dev, i, &byte_val);
160
161 if ((byte_val & mask) == val) {
162 found = 1;
163 }
164
165 if ((status == VL53L1_ERROR_NONE) && (found == 0) && (delay > 0)) {
166 /* Allow for other threads to run */
167 status = VL53L1_WaitMs(dev, delay);
168 }
169
170 /* Update polling time (Compare difference rather than absolute to
171 * negate 32bit wrap around issue)
172 */
173 VL53L1_GetTickCount(¤t_time_ms);
174 dev->new_data_ready_poll_duration_ms = current_time_ms - start_time_ms;
175 }
176
177 if (found == 0 && status == VL53L1_ERROR_NONE) {
178 status = VL53L1_ERROR_TIME_OUT;
179 }
180
181 return status;
182 }
183
