1 /*
2 * Copyright (c) 2022, Prevas A/S
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #define DT_DRV_COMPAT nxp_mcux_qdec
7
8 #include <errno.h>
9 #include <stdint.h>
10
11 #include <fsl_enc.h>
12 #include <fsl_xbara.h>
13
14 #include <zephyr/drivers/pinctrl.h>
15 #include <zephyr/drivers/sensor.h>
16 #include <zephyr/drivers/sensor/qdec_mcux.h>
17 #include <zephyr/drivers/spi.h>
18 #include <zephyr/logging/log.h>
19
20 LOG_MODULE_REGISTER(qdec_mcux, CONFIG_SENSOR_LOG_LEVEL);
21
22 struct qdec_mcux_config {
23 ENC_Type *base;
24 const struct pinctrl_dev_config *pincfg;
25 XBARA_Type *xbar;
26 size_t xbar_maps_len;
27 int xbar_maps[];
28 };
29
30 struct qdec_mcux_data {
31 enc_config_t qdec_config;
32 int32_t position;
33 uint16_t counts_per_revolution;
34 };
35
int_to_work_mode(int32_t val)36 static enc_decoder_work_mode_t int_to_work_mode(int32_t val)
37 {
38 return val == 0 ? kENC_DecoderWorkAsNormalMode :
39 kENC_DecoderWorkAsSignalPhaseCountMode;
40 }
41
qdec_mcux_attr_set(const struct device * dev,enum sensor_channel ch,enum sensor_attribute attr,const struct sensor_value * val)42 static int qdec_mcux_attr_set(const struct device *dev, enum sensor_channel ch,
43 enum sensor_attribute attr, const struct sensor_value *val)
44 {
45 const struct qdec_mcux_config *config = dev->config;
46 struct qdec_mcux_data *data = dev->data;
47
48 if (ch != SENSOR_CHAN_ROTATION) {
49 return -ENOTSUP;
50 }
51
52 switch ((enum sensor_attribute_qdec_mcux) attr) {
53 case SENSOR_ATTR_QDEC_MOD_VAL:
54 if (!IN_RANGE(val->val1, 1, UINT16_MAX)) {
55 LOG_ERR("SENSOR_ATTR_QDEC_MOD_VAL value invalid");
56 return -EINVAL;
57 }
58 data->counts_per_revolution = val->val1;
59 return 0;
60 case SENSOR_ATTR_QDEC_ENABLE_SINGLE_PHASE:
61 data->qdec_config.decoderWorkMode =
62 int_to_work_mode(val->val1);
63
64 WRITE_BIT(config->base->CTRL, ENC_CTRL_PH1_SHIFT, val->val1);
65 return 0;
66 default:
67 return -ENOTSUP;
68 }
69 }
70
qdec_mcux_attr_get(const struct device * dev,enum sensor_channel ch,enum sensor_attribute attr,struct sensor_value * val)71 static int qdec_mcux_attr_get(const struct device *dev, enum sensor_channel ch,
72 enum sensor_attribute attr, struct sensor_value *val)
73 {
74 struct qdec_mcux_data *data = dev->data;
75
76 if (ch != SENSOR_CHAN_ROTATION) {
77 return -ENOTSUP;
78 }
79
80 switch ((enum sensor_attribute_qdec_mcux) attr) {
81 case SENSOR_ATTR_QDEC_MOD_VAL:
82 val->val1 = data->counts_per_revolution;
83 return 0;
84 case SENSOR_ATTR_QDEC_ENABLE_SINGLE_PHASE:
85 val->val1 = data->qdec_config.decoderWorkMode ==
86 kENC_DecoderWorkAsNormalMode ? 0 : 1;
87 return 0;
88 default:
89 return -ENOTSUP;
90 }
91 }
92
qdec_mcux_fetch(const struct device * dev,enum sensor_channel ch)93 static int qdec_mcux_fetch(const struct device *dev, enum sensor_channel ch)
94 {
95 const struct qdec_mcux_config *config = dev->config;
96 struct qdec_mcux_data *data = dev->data;
97
98 if (ch != SENSOR_CHAN_ALL) {
99 return -ENOTSUP;
100 }
101
102 /* Read position */
103 data->position = ENC_GetPositionValue(config->base);
104
105 LOG_DBG("pos %d", data->position);
106
107 return 0;
108 }
109
qdec_mcux_ch_get(const struct device * dev,enum sensor_channel ch,struct sensor_value * val)110 static int qdec_mcux_ch_get(const struct device *dev, enum sensor_channel ch,
111 struct sensor_value *val)
112 {
113 struct qdec_mcux_data *data = dev->data;
114
115 switch (ch) {
116 case SENSOR_CHAN_ROTATION:
117 sensor_value_from_float(val, (data->position * 360.0f)
118 / data->counts_per_revolution);
119 break;
120 default:
121 return -ENOTSUP;
122 }
123
124 return 0;
125 }
126
127 static DEVICE_API(sensor, qdec_mcux_api) = {
128 .attr_set = &qdec_mcux_attr_set,
129 .attr_get = &qdec_mcux_attr_get,
130 .sample_fetch = &qdec_mcux_fetch,
131 .channel_get = &qdec_mcux_ch_get,
132 };
133
init_inputs(const struct device * dev)134 static void init_inputs(const struct device *dev)
135 {
136 int i;
137 const struct qdec_mcux_config *config = dev->config;
138
139 i = pinctrl_apply_state(config->pincfg, PINCTRL_STATE_DEFAULT);
140 assert(i == 0);
141
142 /* Quadrature Encoder inputs are only accessible via crossbar */
143 XBARA_Init(config->xbar);
144 for (i = 0; i < config->xbar_maps_len; i += 2) {
145 XBARA_SetSignalsConnection(config->xbar, config->xbar_maps[i],
146 config->xbar_maps[i + 1]);
147 }
148 }
149
150 #define XBAR_PHANDLE(n) DT_INST_PHANDLE(n, xbar)
151
152 #define QDEC_CHECK_COND(n, p, min, max) \
153 COND_CODE_1(DT_INST_NODE_HAS_PROP(n, p), ( \
154 BUILD_ASSERT(IN_RANGE(DT_INST_PROP(n, p), min, max), \
155 STRINGIFY(p) " value is out of range")), ())
156
157 #define QDEC_SET_COND(n, v, p) \
158 COND_CODE_1(DT_INST_NODE_HAS_PROP(n, p), (v = DT_INST_PROP(n, p)), ())
159
160 #define QDEC_MCUX_INIT(n) \
161 \
162 BUILD_ASSERT((DT_PROP_LEN(XBAR_PHANDLE(n), xbar_maps) % 2) == 0, \
163 "xbar_maps length must be an even number"); \
164 QDEC_CHECK_COND(n, counts_per_revolution, 1, UINT16_MAX); \
165 QDEC_CHECK_COND(n, filter_sample_period, 0, UINT8_MAX); \
166 \
167 static struct qdec_mcux_data qdec_mcux_##n##_data = { \
168 .counts_per_revolution = DT_INST_PROP(n, counts_per_revolution) \
169 }; \
170 \
171 PINCTRL_DT_INST_DEFINE(n); \
172 \
173 static const struct qdec_mcux_config qdec_mcux_##n##_config = { \
174 .base = (ENC_Type *)DT_INST_REG_ADDR(n), \
175 .xbar = (XBARA_Type *)DT_REG_ADDR(XBAR_PHANDLE(n)), \
176 .xbar_maps_len = DT_PROP_LEN(XBAR_PHANDLE(n), xbar_maps), \
177 .xbar_maps = DT_PROP(XBAR_PHANDLE(n), xbar_maps), \
178 .pincfg = PINCTRL_DT_INST_DEV_CONFIG_GET(n), \
179 }; \
180 \
181 static int qdec_mcux_##n##_init(const struct device *dev) \
182 { \
183 const struct qdec_mcux_config *config = dev->config; \
184 struct qdec_mcux_data *data = dev->data; \
185 \
186 LOG_DBG("Initializing %s", dev->name); \
187 \
188 init_inputs(dev); \
189 \
190 ENC_GetDefaultConfig(&data->qdec_config); \
191 data->qdec_config.decoderWorkMode = int_to_work_mode( \
192 DT_INST_PROP(n, single_phase_mode)); \
193 QDEC_SET_COND(n, data->qdec_config.filterCount, filter_count); \
194 QDEC_SET_COND(n, data->qdec_config.filterSamplePeriod, \
195 filter_sample_period); \
196 LOG_DBG("Latency is %u filter clock cycles + 2 IPBus clock " \
197 "periods", data->qdec_config.filterSamplePeriod * \
198 (data->qdec_config.filterCount + 3)); \
199 ENC_Init(config->base, &data->qdec_config); \
200 \
201 /* Update the position counter with initial value. */ \
202 ENC_DoSoftwareLoadInitialPositionValue(config->base); \
203 \
204 return 0; \
205 } \
206 \
207 \
208 SENSOR_DEVICE_DT_INST_DEFINE(n, qdec_mcux_##n##_init, NULL, \
209 &qdec_mcux_##n##_data, &qdec_mcux_##n##_config, \
210 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
211 &qdec_mcux_api); \
212 \
213
214 DT_INST_FOREACH_STATUS_OKAY(QDEC_MCUX_INIT)
215
