1 /*
2 * Copyright (c) 2025 Croxel Inc.
3 * Copyright (c) 2025 CogniPilot Foundation
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/drivers/sensor_clock.h>
9 #include <zephyr/sys/byteorder.h>
10 #include <zephyr/drivers/sensor/pat9136.h>
11
12 #include "pat9136.h"
13 #include "pat9136_reg.h"
14 #include "pat9136_decoder.h"
15
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(PAT9136_DECODER, CONFIG_SENSOR_LOG_LEVEL);
18
19 #define DT_DRV_COMPAT pixart_pat9136
20
pat9136_get_shift(uint16_t channel,uint16_t res_x,uint16_t res_y,int8_t * shift)21 static int pat9136_get_shift(uint16_t channel,
22 uint16_t res_x,
23 uint16_t res_y,
24 int8_t *shift)
25 {
26 /** Table generated based on calculation of required bits:
27 * - resolution_cpi (per datasheet) = (1 + resolution) * 100
28 * - value_mm = value * 25.4 / resolution_cpi
29 * - Bits Required = round_up( Log2(value_mm) )
30 */
31 struct {
32 uint16_t min;
33 uint16_t max;
34 int8_t shift;
35 } const static shift_based_on_ranges[] = {
36 {.min = 0, .max = 0, .shift = 14},
37 {.min = 1, .max = 1, .shift = 13},
38 {.min = 2, .max = 3, .shift = 12},
39 {.min = 4, .max = 7, .shift = 11},
40 {.min = 8, .max = 15, .shift = 10},
41 {.min = 16, .max = 31, .shift = 9},
42 {.min = 32, .max = 63, .shift = 8},
43 {.min = 64, .max = 127, .shift = 7},
44 {.min = 128, .max = 199, .shift = 6},
45 };
46 /** Going with lowest resolution to be able to represent biggest value */
47 uint16_t resolution = MIN(res_x, res_y);
48
49 switch (channel) {
50 case SENSOR_CHAN_POS_DX:
51 case SENSOR_CHAN_POS_DY:
52 case SENSOR_CHAN_POS_DXYZ:
53 *shift = 31;
54 return 0;
55 case SENSOR_CHAN_POS_DX_MM:
56 case SENSOR_CHAN_POS_DY_MM:
57 case SENSOR_CHAN_POS_DXYZ_MM:
58 for (size_t i = 0 ; i < ARRAY_SIZE(shift_based_on_ranges) ; i++) {
59 if (resolution >= shift_based_on_ranges[i].min &&
60 resolution <= shift_based_on_ranges[i].max) {
61 *shift = shift_based_on_ranges[i].shift;
62 return 0;
63 }
64 }
65 default:
66 return -EINVAL;
67 }
68
69 return -EIO;
70 }
71
pat9136_convert_raw_to_q31(struct pat9136_encoded_data * edata,uint16_t chan,int32_t reading,q31_t * out)72 static void pat9136_convert_raw_to_q31(struct pat9136_encoded_data *edata,
73 uint16_t chan,
74 int32_t reading,
75 q31_t *out)
76 {
77 int8_t shift;
78 uint32_t resolution_setting = MIN(edata->header.resolution.x,
79 edata->header.resolution.y);
80 int64_t intermediate;
81
82 (void)pat9136_get_shift(chan,
83 resolution_setting,
84 resolution_setting,
85 &shift);
86
87 switch (chan) {
88 case SENSOR_CHAN_POS_DX:
89 case SENSOR_CHAN_POS_DY:
90 *out = (q31_t)reading;
91 return;
92 case SENSOR_CHAN_POS_DX_MM:
93 case SENSOR_CHAN_POS_DY_MM: {
94 uint32_t resolution = (chan == SENSOR_CHAN_POS_DX_MM) ?
95 (edata->header.resolution.x + 1) * 100 :
96 (edata->header.resolution.y + 1) * 100;
97
98 intermediate = (((int64_t)reading * INT64_C(1000000) * 25.4));
99 intermediate /= resolution;
100
101 intermediate *= ((int64_t)INT32_MAX + 1) / ((1 << shift) * INT64_C(1000000));
102
103 *out = CLAMP(intermediate, INT32_MIN, INT32_MAX);
104
105 return;
106 }
107 default:
108 CODE_UNREACHABLE;
109 }
110 }
111
pat9136_encode_channel(uint16_t chan)112 uint8_t pat9136_encode_channel(uint16_t chan)
113 {
114 switch (chan) {
115 case SENSOR_CHAN_POS_DX:
116 return BIT(0);
117 case SENSOR_CHAN_POS_DY:
118 return BIT(1);
119 case SENSOR_CHAN_POS_DXYZ:
120 return BIT(2);
121 case SENSOR_CHAN_POS_DX_MM:
122 return BIT(3);
123 case SENSOR_CHAN_POS_DY_MM:
124 return BIT(4);
125 case SENSOR_CHAN_POS_DXYZ_MM:
126 return BIT(5);
127 case SENSOR_CHAN_ALL:
128 return BIT_MASK(6);
129 default:
130 return 0;
131 }
132 }
133
is_data_valid(const struct pat9136_encoded_data * edata)134 static bool is_data_valid(const struct pat9136_encoded_data *edata)
135 {
136 if (!REG_MOTION_DETECTED(edata->motion)) {
137 LOG_WRN("Invalid data - No motion detected");
138 return false;
139 }
140
141 if (!REG_OBSERVATION_READ_IS_VALID(edata->observation)) {
142 LOG_WRN("Invalid data - Observation read is not valid");
143 return false;
144 }
145
146 return true;
147 }
148
pat9136_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)149 static int pat9136_decoder_get_frame_count(const uint8_t *buffer,
150 struct sensor_chan_spec chan_spec,
151 uint16_t *frame_count)
152 {
153 struct pat9136_encoded_data *edata = (struct pat9136_encoded_data *)buffer;
154
155 if (chan_spec.chan_idx != 0) {
156 return -ENOTSUP;
157 }
158
159 uint8_t channel_request = pat9136_encode_channel(chan_spec.chan_type);
160
161 if (((edata->header.channels & channel_request) != channel_request) ||
162 !is_data_valid(edata)) {
163 return -ENODATA;
164 }
165
166 switch (chan_spec.chan_type) {
167 case SENSOR_CHAN_POS_DX:
168 case SENSOR_CHAN_POS_DY:
169 case SENSOR_CHAN_POS_DXYZ:
170 case SENSOR_CHAN_POS_DX_MM:
171 case SENSOR_CHAN_POS_DY_MM:
172 case SENSOR_CHAN_POS_DXYZ_MM:
173 *frame_count = 1;
174 return 0;
175 default:
176 return -ENOTSUP;
177 }
178
179 return -1;
180 }
181
pat9136_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)182 static int pat9136_decoder_get_size_info(struct sensor_chan_spec chan_spec,
183 size_t *base_size,
184 size_t *frame_size)
185 {
186 switch (chan_spec.chan_type) {
187 case SENSOR_CHAN_POS_DX:
188 case SENSOR_CHAN_POS_DY:
189 case SENSOR_CHAN_POS_DX_MM:
190 case SENSOR_CHAN_POS_DY_MM:
191 *base_size = sizeof(struct sensor_q31_data);
192 *frame_size = sizeof(struct sensor_q31_sample_data);
193 return 0;
194 case SENSOR_CHAN_POS_DXYZ:
195 case SENSOR_CHAN_POS_DXYZ_MM:
196 *base_size = sizeof(struct sensor_three_axis_data);
197 *frame_size = sizeof(struct sensor_three_axis_data);
198 return 0;
199 default:
200 return -ENOTSUP;
201 }
202 }
203
pat9136_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)204 static int pat9136_decoder_decode(const uint8_t *buffer,
205 struct sensor_chan_spec chan_spec,
206 uint32_t *fit,
207 uint16_t max_count,
208 void *data_out)
209 {
210 struct pat9136_encoded_data *edata = (struct pat9136_encoded_data *)buffer;
211 uint8_t channel_request;
212
213 if (*fit != 0) {
214 return 0;
215 }
216
217 if (max_count == 0 || chan_spec.chan_idx != 0) {
218 return -EINVAL;
219 }
220
221 switch (chan_spec.chan_type) {
222 case SENSOR_CHAN_POS_DX_MM:
223 case SENSOR_CHAN_POS_DY_MM:
224 case SENSOR_CHAN_POS_DX:
225 case SENSOR_CHAN_POS_DY: {
226 channel_request = pat9136_encode_channel(chan_spec.chan_type);
227 if (((edata->header.channels & channel_request) != channel_request) ||
228 !is_data_valid(edata)) {
229 LOG_ERR("No data available");
230 return -ENODATA;
231 }
232
233 struct sensor_q31_data *out = (struct sensor_q31_data *)data_out;
234 int16_t raw_value = (chan_spec.chan_type == SENSOR_CHAN_POS_DX ||
235 chan_spec.chan_type == SENSOR_CHAN_POS_DX_MM) ?
236 edata->delta.x :
237 edata->delta.y;
238
239 out->header.base_timestamp_ns = edata->header.timestamp;
240 out->header.reading_count = 1;
241
242 (void)pat9136_get_shift(chan_spec.chan_type,
243 edata->header.resolution.x,
244 edata->header.resolution.y,
245 &out->shift);
246
247 (void)pat9136_convert_raw_to_q31(edata,
248 chan_spec.chan_type,
249 raw_value,
250 &out->readings->value);
251
252 *fit = 1;
253 return 1;
254 }
255 case SENSOR_CHAN_POS_DXYZ_MM:
256 case SENSOR_CHAN_POS_DXYZ: {
257 channel_request = pat9136_encode_channel(chan_spec.chan_type);
258 if (((edata->header.channels & channel_request) != channel_request) ||
259 !is_data_valid(edata)) {
260 LOG_ERR("No data available");
261 return -ENODATA;
262 }
263
264 struct sensor_three_axis_data *out = (struct sensor_three_axis_data *)data_out;
265
266 out->header.base_timestamp_ns = edata->header.timestamp;
267 out->header.reading_count = 1;
268
269 (void)pat9136_get_shift(chan_spec.chan_type,
270 edata->header.resolution.x,
271 edata->header.resolution.y,
272 &out->shift);
273
274 (void)pat9136_convert_raw_to_q31(edata,
275 chan_spec.chan_type - 3,
276 edata->delta.x,
277 &out->readings[0].x);
278 (void)pat9136_convert_raw_to_q31(edata,
279 chan_spec.chan_type - 2,
280 edata->delta.y,
281 &out->readings[0].y);
282 out->readings[0].z = 0;
283
284 *fit = 1;
285 return 1;
286 }
287 default:
288 return -EINVAL;
289 }
290
291 return -1;
292 }
293
pat9136_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)294 static bool pat9136_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
295 {
296 struct pat9136_encoded_data *edata = (struct pat9136_encoded_data *)buffer;
297
298 switch (trigger) {
299 case SENSOR_TRIG_DATA_READY:
300 return edata->header.events.drdy;
301 case SENSOR_TRIG_MOTION:
302 return edata->header.events.motion;
303 default:
304 return false;
305 }
306 }
307
308 SENSOR_DECODER_API_DT_DEFINE() = {
309 .get_frame_count = pat9136_decoder_get_frame_count,
310 .get_size_info = pat9136_decoder_get_size_info,
311 .decode = pat9136_decoder_decode,
312 .has_trigger = pat9136_decoder_has_trigger,
313 };
314
pat9136_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)315 int pat9136_get_decoder(const struct device *dev,
316 const struct sensor_decoder_api **decoder)
317 {
318 ARG_UNUSED(dev);
319 *decoder = &SENSOR_DECODER_NAME();
320
321 return 0;
322 }
323
pat9136_encode(const struct device * dev,const struct sensor_chan_spec * const channels,size_t num_channels,uint8_t * buf)324 int pat9136_encode(const struct device *dev,
325 const struct sensor_chan_spec *const channels,
326 size_t num_channels,
327 uint8_t *buf)
328 {
329 struct pat9136_encoded_data *edata = (struct pat9136_encoded_data *)buf;
330 uint64_t cycles;
331 int err;
332
333 edata->header.channels = 0;
334 edata->header.events.drdy = 0;
335 edata->header.events.motion = 0;
336
337 for (size_t i = 0 ; i < num_channels; i++) {
338 edata->header.channels |= pat9136_encode_channel(channels[i].chan_type);
339 }
340
341 err = sensor_clock_get_cycles(&cycles);
342 if (err != 0) {
343 return err;
344 }
345
346 edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
347
348 return 0;
349 }
350