1 /*
2  * Copyright (c) 2006-2023, RT-Thread Development Team
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  *
6  * Change Logs:
7  * Date           Author       Notes
8  * 2019-01-31     flybreak     first version
9  * 2020-02-22     luhuadong    support custom commands
10  * 2022-12-17     Meco Man     re-implement sensor framework
11  */
12 
13 #include <drivers/sensor_v2.h>
14 
15 #define DBG_TAG  "sensor_v2"
16 #define DBG_LVL DBG_INFO
17 #include <rtdbg.h>
18 
19 #include <string.h>
20 
21 static char *const sensor_name_str[] =
22 {
23     "None",
24     "ac-",       /* Accelerometer     */
25     "gy-",       /* Gyroscope         */
26     "ma-",       /* Magnetometer      */
27     "tm-",       /* Temperature       */
28     "hm-",       /* Relative Humidity */
29     "br-",       /* Barometer         */
30     "li-",       /* Ambient light     */
31     "pr-",       /* Proximity         */
32     "hr-",       /* Heart Rate        */
33     "tv-",       /* TVOC Level        */
34     "ni-",       /* Noise Loudness    */
35     "st-",       /* Step sensor       */
36     "fr-",       /* Force sensor      */
37     "du-",       /* Dust sensor       */
38     "ec-",       /* eCO2 sensor       */
39     "gn-",       /* GPS/GNSS sensor   */
40     "tf-",       /* TOF sensor        */
41     "sp-",       /* SpO2 sensor       */
42     "ia-",       /* IAQ sensor        */
43     "et-",       /* EtOH sensor       */
44     "bp-",       /* Blood Pressure    */
45     RT_NULL
46 };
47 
48 /* sensor interrupt handler function */
_sensor_cb(rt_sensor_t sen)49 static void _sensor_cb(rt_sensor_t sen)
50 {
51     if (sen->parent.rx_indicate == RT_NULL)
52     {
53         return;
54     }
55 
56     if (sen->irq_handle != RT_NULL)
57     {
58         sen->irq_handle(sen);
59     }
60 
61     /* The buffer is not empty. Read the data in the buffer first */
62     if (sen->data_len > 0)
63     {
64         sen->parent.rx_indicate(&sen->parent, sen->data_len / sizeof(struct rt_sensor_data));
65     }
66     else if (RT_SENSOR_MODE_GET_FETCH(sen->info.mode) == RT_SENSOR_MODE_FETCH_INT)
67     {
68         /* The interrupt mode only produces one data at a time */
69         sen->parent.rx_indicate(&sen->parent, 1);
70     }
71     else if (RT_SENSOR_MODE_GET_FETCH(sen->info.mode) == RT_SENSOR_MODE_FETCH_FIFO)
72     {
73         sen->parent.rx_indicate(&sen->parent, sen->info.fifo_max);
74     }
75 }
76 
77 /* ISR for sensor interrupt */
_irq_callback(void * args)78 static void _irq_callback(void *args)
79 {
80     rt_sensor_t sensor = (rt_sensor_t)args;
81     rt_uint8_t i;
82 
83     if (sensor->module)
84     {
85         /* Invoke a callback for all sensors in the module */
86         for (i = 0; i < sensor->module->sen_num; i++)
87         {
88             _sensor_cb(sensor->module->sen[i]);
89         }
90     }
91     else
92     {
93         _sensor_cb(sensor);
94     }
95 }
96 
97 /* Sensor interrupt initialization function */
_sensor_irq_init(rt_sensor_t sensor)98 static rt_err_t _sensor_irq_init(rt_sensor_t sensor)
99 {
100     if (sensor->config.irq_pin.pin == PIN_IRQ_PIN_NONE)
101     {
102         return -RT_EINVAL;
103     }
104 
105     rt_pin_mode(sensor->config.irq_pin.pin, sensor->config.irq_pin.mode);
106 
107     if (sensor->config.irq_pin.mode == PIN_MODE_INPUT_PULLDOWN)
108     {
109         rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_RISING, _irq_callback, (void *)sensor);
110     }
111     else if (sensor->config.irq_pin.mode == PIN_MODE_INPUT_PULLUP)
112     {
113         rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_FALLING, _irq_callback, (void *)sensor);
114     }
115     else if (sensor->config.irq_pin.mode == PIN_MODE_INPUT)
116     {
117         rt_pin_attach_irq(sensor->config.irq_pin.pin, PIN_IRQ_MODE_RISING_FALLING, _irq_callback, (void *)sensor);
118     }
119 
120     rt_pin_irq_enable(sensor->config.irq_pin.pin, RT_TRUE);
121 
122     LOG_I("interrupt init success");
123 
124     return 0;
125 }
126 
127 /* sensor local ops */
_local_fetch_data(rt_sensor_t sensor,rt_sensor_data_t buf,rt_size_t len)128 static rt_ssize_t _local_fetch_data(rt_sensor_t sensor, rt_sensor_data_t buf, rt_size_t len)
129 {
130     LOG_D("Undefined fetch_data");
131     return -RT_EINVAL;
132 }
_local_control(rt_sensor_t sensor,int cmd,void * arg)133 static rt_err_t _local_control(rt_sensor_t sensor, int cmd, void *arg)
134 {
135     LOG_D("Undefined control");
136     return -RT_EINVAL;
137 }
138 static struct rt_sensor_ops local_ops =
139 {
140     .fetch_data = _local_fetch_data,
141     .control = _local_control
142 };
143 
144 /* RT-Thread Device Interface */
_sensor_open(rt_device_t dev,rt_uint16_t oflag)145 static rt_err_t _sensor_open(rt_device_t dev, rt_uint16_t oflag)
146 {
147     rt_sensor_t sensor = (rt_sensor_t)dev;
148     RT_ASSERT(dev != RT_NULL);
149     rt_err_t res = RT_EOK;
150     rt_err_t (*local_ctrl)(rt_sensor_t sensor, int cmd, void *arg) =  _local_control;
151 
152     if (sensor->module)
153     {
154         /* take the module mutex */
155         rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER);
156     }
157 
158     if (sensor->module != RT_NULL && sensor->info.fifo_max > 0 && sensor->data_buf == RT_NULL)
159     {
160         /* Allocate memory for the sensor buffer */
161         sensor->data_buf = rt_malloc(sizeof(struct rt_sensor_data) * sensor->info.fifo_max);
162         if (sensor->data_buf == RT_NULL)
163         {
164             res = -RT_ENOMEM;
165             goto __exit;
166         }
167     }
168     if (sensor->ops->control != RT_NULL)
169     {
170         local_ctrl = sensor->ops->control;
171     }
172 
173     if (oflag & RT_DEVICE_FLAG_RDONLY && dev->flag & RT_DEVICE_FLAG_RDONLY)
174     {
175         /* If polling mode is supported, configure it to polling mode */
176         if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_FETCH_MODE, (void *)RT_SENSOR_MODE_FETCH_POLLING) == RT_EOK)
177         {
178             RT_SENSOR_MODE_SET_FETCH(sensor->info.mode, RT_SENSOR_MODE_FETCH_POLLING);
179         }
180     }
181     else if (oflag & RT_DEVICE_FLAG_INT_RX && dev->flag & RT_DEVICE_FLAG_INT_RX)
182     {
183         /* If interrupt mode is supported, configure it to interrupt mode */
184         if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_FETCH_MODE, (void *)RT_SENSOR_MODE_FETCH_INT) == RT_EOK)
185         {
186             /* Initialization sensor interrupt */
187             _sensor_irq_init(sensor);
188             RT_SENSOR_MODE_SET_FETCH(sensor->info.mode, RT_SENSOR_MODE_FETCH_INT);
189         }
190     }
191     else if (oflag & RT_DEVICE_FLAG_FIFO_RX && dev->flag & RT_DEVICE_FLAG_FIFO_RX)
192     {
193         /* If fifo mode is supported, configure it to fifo mode */
194         if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_FETCH_MODE, (void *)RT_SENSOR_MODE_FETCH_FIFO) == RT_EOK)
195         {
196             /* Initialization sensor interrupt */
197             _sensor_irq_init(sensor);
198             RT_SENSOR_MODE_SET_FETCH(sensor->info.mode, RT_SENSOR_MODE_FETCH_FIFO);
199         }
200     }
201     else
202     {
203         res = -RT_EINVAL;
204         goto __exit;
205     }
206 
207     /* Configure power mode to highest mode */
208     if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_POWER_MODE, (void *)RT_SENSOR_MODE_POWER_HIGHEST) == RT_EOK)
209     {
210         RT_SENSOR_MODE_SET_POWER(sensor->info.mode, RT_SENSOR_MODE_POWER_HIGHEST);
211     }
212 
213     /* Configure accuracy mode to highest mode */
214     if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_ACCURACY_MODE, (void *)RT_SENSOR_MODE_ACCURACY_HIGHEST) == RT_EOK)
215     {
216         RT_SENSOR_MODE_SET_ACCURACY(sensor->info.mode, RT_SENSOR_MODE_ACCURACY_HIGHEST);
217     }
218 
219 __exit:
220     if (sensor->module)
221     {
222         /* release the module mutex */
223         rt_mutex_release(sensor->module->lock);
224     }
225 
226     return res;
227 }
228 
_sensor_close(rt_device_t dev)229 static rt_err_t _sensor_close(rt_device_t dev)
230 {
231     rt_sensor_t sensor = (rt_sensor_t)dev;
232     rt_err_t (*local_ctrl)(rt_sensor_t sensor, int cmd, void *arg) = _local_control;
233     int i;
234 
235     RT_ASSERT(dev != RT_NULL);
236 
237     if (sensor->module)
238     {
239         rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER);
240     }
241     if (sensor->ops->control != RT_NULL)
242     {
243         local_ctrl = sensor->ops->control;
244     }
245 
246     /* Configure power mode to power down mode */
247     if (local_ctrl(sensor, RT_SENSOR_CTRL_SET_POWER_MODE, (void *)RT_SENSOR_MODE_POWER_DOWN) == RT_EOK)
248     {
249         RT_SENSOR_MODE_SET_POWER(sensor->info.mode, RT_SENSOR_MODE_POWER_DOWN);
250     }
251 
252     if (sensor->module != RT_NULL && sensor->info.fifo_max > 0 && sensor->data_buf != RT_NULL)
253     {
254         for (i = 0; i < sensor->module->sen_num; i ++)
255         {
256             if (sensor->module->sen[i]->parent.ref_count > 0)
257                 goto __exit;
258         }
259 
260         /* Free memory for the sensor buffer */
261         for (i = 0; i < sensor->module->sen_num; i ++)
262         {
263             if (sensor->module->sen[i]->data_buf)
264             {
265                 rt_free(sensor->module->sen[i]->data_buf);
266                 sensor->module->sen[i]->data_buf = RT_NULL;
267             }
268         }
269     }
270     if (RT_SENSOR_MODE_GET_FETCH(sensor->info.mode) != RT_SENSOR_MODE_FETCH_POLLING)
271     {
272         /* Sensor disable interrupt */
273         if (sensor->config.irq_pin.pin != PIN_IRQ_PIN_NONE)
274         {
275             rt_pin_irq_enable(sensor->config.irq_pin.pin, RT_FALSE);
276         }
277     }
278 
279 __exit:
280     if (sensor->module)
281     {
282         rt_mutex_release(sensor->module->lock);
283     }
284 
285     return RT_EOK;
286 }
287 
_sensor_read(rt_device_t dev,rt_off_t pos,void * buf,rt_size_t len)288 static rt_ssize_t _sensor_read(rt_device_t dev, rt_off_t pos, void *buf, rt_size_t len)
289 {
290     rt_sensor_t sensor = (rt_sensor_t)dev;
291     rt_size_t result = 0;
292     RT_ASSERT(dev != RT_NULL);
293 
294     if (buf == NULL || len == 0)
295     {
296         return 0;
297     }
298 
299     if (sensor->module)
300     {
301         rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER);
302     }
303 
304     /* The buffer is not empty. Read the data in the buffer first */
305     if (sensor->data_len > 0)
306     {
307         if (len > sensor->data_len / sizeof(struct rt_sensor_data))
308         {
309             len = sensor->data_len / sizeof(struct rt_sensor_data);
310         }
311 
312         rt_memcpy(buf, sensor->data_buf, len * sizeof(struct rt_sensor_data));
313 
314         /* Clear the buffer */
315         sensor->data_len = 0;
316         result = len;
317     }
318     else
319     {
320         /* If the buffer is empty, read the data */
321         if (sensor->ops->fetch_data)
322         {
323             result = sensor->ops->fetch_data(sensor, buf, len);
324         }
325     }
326 
327     if (sensor->module)
328     {
329         rt_mutex_release(sensor->module->lock);
330     }
331 
332     return result;
333 }
334 
_sensor_control(rt_device_t dev,int cmd,void * args)335 static rt_err_t _sensor_control(rt_device_t dev, int cmd, void *args)
336 {
337     rt_sensor_t sensor = (rt_sensor_t)dev;
338     rt_err_t result = RT_EOK;
339     RT_ASSERT(dev != RT_NULL);
340     rt_err_t (*local_ctrl)(rt_sensor_t sensor, int cmd, void *arg) = _local_control;
341     rt_uint8_t mode;
342 
343     if (sensor->module)
344     {
345         rt_mutex_take(sensor->module->lock, RT_WAITING_FOREVER);
346     }
347     if (sensor->ops->control != RT_NULL)
348     {
349         local_ctrl = sensor->ops->control;
350     }
351 
352     switch (cmd)
353     {
354         case RT_SENSOR_CTRL_GET_ID:
355             if (args)
356             {
357                 result = local_ctrl(sensor, RT_SENSOR_CTRL_GET_ID, args);
358             }
359             break;
360         case RT_SENSOR_CTRL_SET_ACCURACY_MODE:
361             /* Configuration sensor power mode */
362             mode = (rt_uint32_t)args & 0x000F;
363             if (!(mode == RT_SENSOR_MODE_ACCURACY_HIGHEST || mode == RT_SENSOR_MODE_ACCURACY_HIGH ||\
364                   mode == RT_SENSOR_MODE_ACCURACY_MEDIUM  || mode == RT_SENSOR_MODE_ACCURACY_LOW  ||\
365                   mode == RT_SENSOR_MODE_ACCURACY_LOWEST  || mode == RT_SENSOR_MODE_ACCURACY_NOTRUST))
366             {
367                 LOG_E("sensor accuracy mode illegal: %d", mode);
368                 return -RT_EINVAL;
369             }
370             result = local_ctrl(sensor, RT_SENSOR_CTRL_SET_ACCURACY_MODE, args);
371             if (result == RT_EOK)
372             {
373                 RT_SENSOR_MODE_SET_ACCURACY(sensor->info.mode, mode);
374                 LOG_D("set accuracy mode code: %d", RT_SENSOR_MODE_GET_ACCURACY(sensor->info.mode));
375             }
376             break;
377         case RT_SENSOR_CTRL_SET_POWER_MODE:
378             /* Configuration sensor power mode */
379             mode = (rt_uint32_t)args & 0x000F;
380             if (!(mode == RT_SENSOR_MODE_POWER_HIGHEST || mode == RT_SENSOR_MODE_POWER_HIGH ||\
381                   mode == RT_SENSOR_MODE_POWER_MEDIUM  || mode == RT_SENSOR_MODE_POWER_LOW  ||\
382                   mode == RT_SENSOR_MODE_POWER_LOWEST  || mode == RT_SENSOR_MODE_POWER_DOWN))
383             {
384                 LOG_E("sensor power mode illegal: %d", mode);
385                 return -RT_EINVAL;
386             }
387             result = local_ctrl(sensor, RT_SENSOR_CTRL_SET_POWER_MODE, args);
388             if (result == RT_EOK)
389             {
390                 RT_SENSOR_MODE_SET_POWER(sensor->info.mode, mode);
391                 LOG_D("set power mode code: %d", RT_SENSOR_MODE_GET_POWER(sensor->info.mode));
392             }
393             break;
394         case RT_SENSOR_CTRL_SET_FETCH_MODE:
395             /* Configuration sensor power mode */
396             mode = (rt_uint32_t)args & 0x000F;
397             if (!(mode == RT_SENSOR_MODE_FETCH_POLLING || mode == RT_SENSOR_MODE_FETCH_INT ||\
398                   mode == RT_SENSOR_MODE_FETCH_FIFO))
399             {
400                 LOG_E("sensor fetch data mode illegal: %d", mode);
401                 return -RT_EINVAL;
402             }
403             result = local_ctrl(sensor, RT_SENSOR_CTRL_SET_FETCH_MODE, args);
404             if (result == RT_EOK)
405             {
406                 RT_SENSOR_MODE_SET_FETCH(sensor->info.mode, mode);
407                 LOG_D("set fetch mode code: %d", RT_SENSOR_MODE_GET_FETCH(sensor->info.mode));
408             }
409             break;
410         case RT_SENSOR_CTRL_SELF_TEST:
411             /* device self test */
412             result = local_ctrl(sensor, RT_SENSOR_CTRL_SELF_TEST, args);
413             break;
414         case RT_SENSOR_CTRL_SOFT_RESET:
415             /* device soft reset */
416             result = local_ctrl(sensor, RT_SENSOR_CTRL_SOFT_RESET, args);
417             break;
418         default:
419             if (cmd > RT_SENSOR_CTRL_USER_CMD_START)
420             {
421                 /* Custom commands */
422                 result = local_ctrl(sensor, cmd, args);
423             }
424             else
425             {
426                 result = -RT_EINVAL;
427             }
428             break;
429     }
430 
431     if (sensor->module)
432     {
433         rt_mutex_release(sensor->module->lock);
434     }
435 
436     return result;
437 }
438 
439 #ifdef RT_USING_DEVICE_OPS
440 const static struct rt_device_ops rt_sensor_ops =
441 {
442     RT_NULL,
443     _sensor_open,
444     _sensor_close,
445     _sensor_read,
446     RT_NULL,
447     _sensor_control
448 };
449 #endif
450 
451 /*
452  * sensor register
453  */
rt_hw_sensor_register(rt_sensor_t sensor,const char * name,rt_uint32_t flag,void * data)454 int rt_hw_sensor_register(rt_sensor_t    sensor,
455                           const char    *name,
456                           rt_uint32_t    flag,
457                           void          *data)
458 {
459     rt_int8_t result;
460     rt_device_t device;
461     RT_ASSERT(sensor != RT_NULL);
462 
463     char *sensor_name = RT_NULL, *device_name = RT_NULL;
464 
465     if (sensor->ops == RT_NULL)
466     {
467         sensor->ops = &local_ops;
468     }
469 
470     /* Add a type name for the sensor device */
471     sensor_name = sensor_name_str[sensor->info.type];
472     device_name = (char *)rt_calloc(1, rt_strlen(sensor_name) + 1 + rt_strlen(name));
473     if (device_name == RT_NULL)
474     {
475         LOG_E("device_name calloc failed!");
476         return -RT_ERROR;
477     }
478 
479     rt_memcpy(device_name, sensor_name, rt_strlen(sensor_name) + 1);
480     strcat(device_name, name);
481 
482     if (sensor->module != RT_NULL && sensor->module->lock == RT_NULL)
483     {
484         /* Create a mutex lock for the module */
485         sensor->module->lock = rt_mutex_create(name, RT_IPC_FLAG_PRIO);
486         if (sensor->module->lock == RT_NULL)
487         {
488             rt_free(device_name);
489             return -RT_ERROR;
490         }
491     }
492 
493     device = &sensor->parent;
494 
495 #ifdef RT_USING_DEVICE_OPS
496     device->ops         = &rt_sensor_ops;
497 #else
498     device->init        = RT_NULL;
499     device->open        = _sensor_open;
500     device->close       = _sensor_close;
501     device->read        = _sensor_read;
502     device->write       = RT_NULL;
503     device->control     = _sensor_control;
504 #endif
505     device->type        = RT_Device_Class_Sensor;
506     device->rx_indicate = RT_NULL;
507     device->tx_complete = RT_NULL;
508     device->user_data   = data;
509 
510     result = rt_device_register(device, device_name, flag | RT_DEVICE_FLAG_STANDALONE);
511     if (result != RT_EOK)
512     {
513         LOG_E("sensor[%s] register err code: %d", device_name, result);
514         rt_free(device_name);
515         return result;
516     }
517 
518     LOG_I("sensor[%s] init success", device_name);
519     rt_free(device_name);
520 
521     return RT_EOK;
522 }
523 
rt_sensor_device_find(const char * name)524 rt_sensor_t rt_sensor_device_find(const char *name)
525 {
526     rt_uint8_t index;
527     char device_name[RT_NAME_MAX];
528     rt_device_t device;
529 
530     for (index = 0; sensor_name_str[index] != RT_NULL; index++)
531     {
532         rt_memset(device_name, 0, sizeof(device_name));
533         rt_snprintf(device_name, sizeof(device_name), "%s%s", sensor_name_str[index], name);
534         device = rt_device_find(device_name);
535         if (device != RT_NULL)
536         {
537             return (rt_sensor_t)device;
538         }
539     }
540 
541     return RT_NULL;
542 }
543