1 /*
2  * Copyright (c) 2024 Cienet
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 
7 #include <zephyr/device.h>
8 #include <zephyr/sys/util.h>
9 #include <zephyr/sys_clock.h>
10 #include <zephyr/drivers/counter.h>
11 #include <zephyr/logging/log.h>
12 #include <zephyr/init.h>
13 
14 LOG_MODULE_REGISTER(sensor_clock, CONFIG_SENSOR_LOG_LEVEL);
15 
16 static const struct device *external_sensor_clock = DEVICE_DT_GET(DT_CHOSEN(zephyr_sensor_clock));
17 static uint32_t freq;
18 
external_sensor_clock_init(void)19 static int external_sensor_clock_init(void)
20 {
21 	int rc;
22 
23 	rc = counter_start(external_sensor_clock);
24 	if (rc != 0) {
25 		LOG_ERR("Failed to start sensor clock counter: %d\n", rc);
26 		return rc;
27 	}
28 
29 	freq = counter_get_frequency(external_sensor_clock);
30 	if (freq == 0) {
31 		LOG_ERR("Sensor clock %s has no fixed frequency\n", external_sensor_clock->name);
32 		return -EINVAL;
33 	}
34 
35 	return 0;
36 }
37 
sensor_clock_get_cycles(uint64_t * cycles)38 int sensor_clock_get_cycles(uint64_t *cycles)
39 {
40 	__ASSERT_NO_MSG(counter_is_counting_up(external_sensor_clock));
41 
42 	int rc;
43 	const struct counter_driver_api *api =
44 		(const struct counter_driver_api *)external_sensor_clock->api;
45 
46 	if (api->get_value_64) {
47 		rc = counter_get_value_64(external_sensor_clock, cycles);
48 	} else {
49 		uint32_t result_32;
50 
51 		rc = counter_get_value(external_sensor_clock, &result_32);
52 		*cycles = (uint64_t)result_32;
53 	}
54 
55 	return rc;
56 }
57 
sensor_clock_cycles_to_ns(uint64_t cycles)58 uint64_t sensor_clock_cycles_to_ns(uint64_t cycles)
59 {
60 	return (cycles * NSEC_PER_SEC) / freq;
61 }
62 
63 SYS_INIT(external_sensor_clock_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
64