1 /*
2 * Arm SCP/MCP Software
3 * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 */
7
8 #include "sensor.h"
9
10 #include <fwk_assert.h>
11 #include <fwk_id.h>
12 #include <fwk_log.h>
13 #include <fwk_mm.h>
14 #include <fwk_status.h>
15 #include <fwk_time.h>
16
17 #include <stdbool.h>
18 #include <stddef.h>
19 #include <stdint.h>
20
21 #ifdef BUILD_HAS_SENSOR_TIMESTAMP
22
sensor_timestamp_dev_init(fwk_id_t id,struct sensor_dev_ctx * ctx)23 int sensor_timestamp_dev_init(fwk_id_t id, struct sensor_dev_ctx *ctx)
24 {
25 ctx->timestamp.timestamp_support = ctx->config->timestamp.timestamp_support;
26 if (ctx->timestamp.timestamp_support) {
27 return sensor_set_timestamp_config(id, &ctx->config->timestamp);
28 }
29 return FWK_SUCCESS;
30 }
31
sensor_set_timestamp_config(fwk_id_t id,const struct mod_sensor_timestamp_info * config)32 int sensor_set_timestamp_config(
33 fwk_id_t id,
34 const struct mod_sensor_timestamp_info *config)
35 {
36 struct sensor_dev_ctx *ctx;
37
38 fwk_assert(!fwk_id_is_equal(id, FWK_ID_NONE));
39 if (config == NULL) {
40 return FWK_E_PARAM;
41 }
42
43 ctx = sensor_get_ctx(id);
44
45 if (!ctx->timestamp.timestamp_support) {
46 return FWK_E_SUPPORT;
47 }
48
49 ctx->timestamp.enabled = config->enabled;
50 ctx->timestamp.exponent = config->exponent;
51
52 return FWK_SUCCESS;
53 }
54
sensor_get_timestamp_config(fwk_id_t id,struct mod_sensor_timestamp_info * config)55 int sensor_get_timestamp_config(
56 fwk_id_t id,
57 struct mod_sensor_timestamp_info *config)
58 {
59 struct sensor_dev_ctx *ctx;
60
61 fwk_assert(!fwk_id_is_equal(id, FWK_ID_NONE));
62 if (config == NULL) {
63 return FWK_E_PARAM;
64 }
65
66 ctx = sensor_get_ctx(id);
67
68 if (!ctx->timestamp.timestamp_support) {
69 return FWK_E_SUPPORT;
70 }
71
72 config->timestamp_support = ctx->timestamp.timestamp_support;
73 config->enabled = ctx->timestamp.enabled;
74 config->exponent = ctx->timestamp.exponent;
75
76 return FWK_SUCCESS;
77 }
78
79 /* Power function implemented for positive integers */
pow_unsigned(uint64_t base,uint64_t exponent)80 static uint64_t pow_unsigned(uint64_t base, uint64_t exponent)
81 {
82 uint64_t result = 1;
83 for (; exponent > 0; exponent--) {
84 result *= base;
85 }
86 return result;
87 }
88
sensor_get_timestamp(fwk_id_t id)89 uint64_t sensor_get_timestamp(fwk_id_t id)
90 {
91 struct sensor_dev_ctx *ctx;
92 uint64_t timestamp;
93 int64_t normalized_exponent;
94
95 ctx = sensor_get_ctx(id);
96
97 if (!ctx->timestamp.enabled) {
98 return 0;
99 }
100
101 timestamp = FWK_NS(fwk_time_current());
102 /*
103 * Exponent is the power-of-10 multiplier that is applied to the
104 * sensor timestamps (timestamp x 10 [timestamp exponent] ) to
105 * represent it in seconds. Since `fwk_time_current()` base time is in
106 * nano seconds, base unit should be changed.
107 */
108 normalized_exponent = ctx->timestamp.exponent + 9;
109 if (normalized_exponent > 0) {
110 return timestamp / pow_unsigned(10, normalized_exponent);
111 }
112 return timestamp / pow_unsigned(10, -normalized_exponent);
113 }
114
115 #endif
116
117 #ifdef BUILD_HAS_SENSOR_MULTI_AXIS
118
sensor_axis_start(fwk_id_t id)119 int sensor_axis_start(fwk_id_t id)
120 {
121 struct sensor_dev_ctx *ctx;
122 struct mod_sensor_axis_info axis_info;
123 enum mod_sensor_type type;
124 unsigned int i;
125 int status;
126
127 ctx = sensor_get_ctx(id);
128
129 if (ctx->driver_api->get_axis_count) {
130 ctx->axis_count =
131 ctx->driver_api->get_axis_count(ctx->config->driver_id);
132 } else {
133 ctx->axis_count = 1;
134 }
135 ctx->last_read.axis_value =
136 fwk_mm_calloc(ctx->axis_count, sizeof(uint64_t));
137 ctx->last_read.axis_count = ctx->axis_count;
138
139 if (ctx->config->trip_point.count > 0) {
140 for (i = 0; i < ctx->axis_count; i++) {
141 status = sensor_get_axis_info(id, i, &axis_info);
142 if (status != FWK_SUCCESS) {
143 return status;
144 }
145
146 /* Save value of first axis type */
147 if (i == 0) {
148 type = axis_info.type;
149 } else if (axis_info.type != type) {
150 ctx->trip_point_ctx->enabled = false;
151
152 /*
153 * Valid situation where axis have different types, for that
154 * reason trip points are disabled for this particular sensor.
155 */
156 FWK_LOG_INFO(
157 "[Sensor] Trip points are disable. Different axis type");
158
159 return FWK_SUCCESS;
160 }
161 }
162
163 ctx->trip_point_ctx->enabled = true;
164 }
165
166 return FWK_SUCCESS;
167 }
168
sensor_get_axis_info(fwk_id_t id,uint32_t axis,struct mod_sensor_axis_info * info)169 int sensor_get_axis_info(
170 fwk_id_t id,
171 uint32_t axis,
172 struct mod_sensor_axis_info *info)
173 {
174 struct sensor_dev_ctx *ctx;
175
176 if (info == NULL) {
177 return FWK_E_PARAM;
178 }
179
180 ctx = sensor_get_ctx(id);
181
182 if (axis > ctx->axis_count) {
183 return FWK_E_PARAM;
184 }
185
186 ctx->driver_api->get_axis_info(ctx->config->driver_id, axis, info);
187 return FWK_SUCCESS;
188 }
189
190 #endif
191