1 /*
2 * Copyright (c) 2025 Croxel Inc.
3 * Copyright (c) 2025 CogniPilot Foundation
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #define DT_DRV_COMPAT brcm_afbr_s50
9
10 #include <zephyr/drivers/sensor_clock.h>
11 #include <api/argus_res.h>
12 #include <zephyr/drivers/sensor/afbr_s50.h>
13
14 #include "afbr_s50_decoder.h"
15
16 #include <zephyr/logging/log.h>
17 LOG_MODULE_REGISTER(AFBR_S50_DECODER, CONFIG_SENSOR_LOG_LEVEL);
18
afbr_s50_encode_channel(uint16_t chan)19 uint8_t afbr_s50_encode_channel(uint16_t chan)
20 {
21 switch (chan) {
22 case SENSOR_CHAN_DISTANCE:
23 return BIT(0);
24 case SENSOR_CHAN_AFBR_S50_PIXELS:
25 return BIT(1);
26 default:
27 return 0;
28 }
29 }
30
afbr_s50_encode_event(enum sensor_trigger_type trigger)31 uint8_t afbr_s50_encode_event(enum sensor_trigger_type trigger)
32 {
33 if (trigger == SENSOR_TRIG_DATA_READY) {
34 return BIT(0);
35 }
36
37 return 0;
38 }
39
afbr_s50_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)40 static int afbr_s50_decoder_get_frame_count(const uint8_t *buffer,
41 struct sensor_chan_spec chan_spec,
42 uint16_t *frame_count)
43 {
44 const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
45
46 if (chan_spec.chan_idx != 0) {
47 return -ENOTSUP;
48 }
49
50 switch (chan_spec.chan_type) {
51 case SENSOR_CHAN_DISTANCE:
52 case SENSOR_CHAN_AFBR_S50_PIXELS:
53 if (edata->header.channels & afbr_s50_encode_channel(chan_spec.chan_type)) {
54 *frame_count = 1;
55 return 0;
56 }
57 break;
58 default:
59 break;
60 }
61
62 *frame_count = 0;
63 return 0;
64 }
65
afbr_s50_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)66 static int afbr_s50_decoder_get_size_info(struct sensor_chan_spec chan_spec,
67 size_t *base_size,
68 size_t *frame_size)
69 {
70 switch (chan_spec.chan_type) {
71 case SENSOR_CHAN_DISTANCE:
72 *base_size = sizeof(struct sensor_q31_data);
73 *frame_size = sizeof(struct sensor_q31_sample_data);
74 return 0;
75 case SENSOR_CHAN_AFBR_S50_PIXELS:
76 *base_size = sizeof(struct sensor_q31_data) +
77 31 * sizeof(struct sensor_q31_sample_data);
78 *frame_size = 32 * sizeof(struct sensor_q31_sample_data);
79 return 0;
80 default:
81 return -ENOTSUP;
82 }
83 }
84
afbr_s50_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)85 static int afbr_s50_decoder_decode(const uint8_t *buffer,
86 struct sensor_chan_spec chan_spec,
87 uint32_t *fit,
88 uint16_t max_count,
89 void *data_out)
90 {
91 const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
92
93 if (*fit != 0) {
94 return 0;
95 }
96
97 if (max_count == 0 || chan_spec.chan_idx != 0) {
98 return -EINVAL;
99 }
100
101 switch (chan_spec.chan_type) {
102 case SENSOR_CHAN_DISTANCE: {
103 struct sensor_q31_data *out = data_out;
104
105 if ((edata->header.channels & afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE)) == 0) {
106 return -ENODATA;
107 }
108
109 out->header.base_timestamp_ns = edata->header.timestamp;
110 out->header.reading_count = 1;
111
112 /* Result comes encoded in Q9.22 format */
113 out->shift = 9;
114 out->readings[0].value = edata->payload.Bin.Range;
115
116 *fit = 1;
117 return 1;
118 }
119 case SENSOR_CHAN_AFBR_S50_PIXELS: {
120 struct sensor_q31_data *out = data_out;
121
122 if ((edata->header.channels &
123 afbr_s50_encode_channel(SENSOR_CHAN_AFBR_S50_PIXELS)) == 0) {
124 return -ENODATA;
125 }
126
127 out->header.base_timestamp_ns = edata->header.timestamp;
128 out->header.reading_count = 32;
129 /* Result comes encoded in Q9.22 format */
130 out->shift = 9;
131
132 for (size_t i = 0 ; i < 32 ; i++) {
133 if (edata->payload.Pixels[i].Amplitude == 0xFFFF ||
134 edata->payload.Pixels[i].Status != PIXEL_OK) {
135 LOG_DBG("Invalid pixel: %zu, Amplitude: %u, Status: %u", i,
136 edata->payload.Pixels[i].Amplitude,
137 edata->payload.Pixels[i].Status);
138
139 out->readings[i].value = AFBR_PIXEL_INVALID_VALUE;
140 } else {
141 out->readings[i].value = edata->payload.Pixels[i].Range;
142 }
143 out->readings[i].timestamp_delta = 0;
144 }
145
146 *fit = 1;
147 return 1;
148 }
149 default:
150 return -EINVAL;
151 }
152 }
153
afbr_s50_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)154 static bool afbr_s50_decoder_has_trigger(const uint8_t *buffer,
155 enum sensor_trigger_type trigger)
156 {
157 const struct afbr_s50_edata *edata = (const struct afbr_s50_edata *)buffer;
158
159 if (trigger == SENSOR_TRIG_DATA_READY) {
160 return edata->header.events & afbr_s50_encode_event(SENSOR_TRIG_DATA_READY);
161 } else {
162 return false;
163 }
164 }
165
166 SENSOR_DECODER_API_DT_DEFINE() = {
167 .get_frame_count = afbr_s50_decoder_get_frame_count,
168 .get_size_info = afbr_s50_decoder_get_size_info,
169 .decode = afbr_s50_decoder_decode,
170 .has_trigger = afbr_s50_decoder_has_trigger,
171 };
172
afbr_s50_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)173 int afbr_s50_get_decoder(const struct device *dev,
174 const struct sensor_decoder_api **decoder)
175 {
176 ARG_UNUSED(dev);
177 *decoder = &SENSOR_DECODER_NAME();
178
179 return 0;
180 }
181