1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
4 *
5 * Author: Kay Guo <kay.guo@rock-chips.com>
6 */
7 #include <linux/kernel.h>
8 #include "da228e_core.h"
9
10 #define DA228E_OFFSET_THRESHOLD 20
11 #define PEAK_LVL 800
12 #define STICK_LSB 2000
13 #define AIX_HISTORY_SIZE 20
14
15 #define TEMP_CALIBRATE_STATIC_THRESHOLD 60
16 #define TEMP_CALIBRATE_STATIC_COUNT 12
17
18 static int z_offset;
19
squareRoot(int val)20 static int squareRoot(int val)
21 {
22 int ret = 0, x;
23 int shift;
24
25 if (val < 0)
26 return 0;
27
28 for (shift = 0; shift < 32; shift += 2) {
29 x = 0x40000000l >> shift;
30 if ((x+ret) <= val) {
31 val -= x + ret;
32 ret = (ret >> 1) | x;
33 } else
34 ret = ret >> 1;
35 }
36
37 return ret;
38 }
39
da228e_temp_calibrate_detect_static(short x,short y,short z)40 static int da228e_temp_calibrate_detect_static(short x, short y, short z)
41 {
42 static int count_static;
43 static short temp_x[TEMP_CALIBRATE_STATIC_COUNT];
44 static short temp_y[TEMP_CALIBRATE_STATIC_COUNT];
45 static short temp_z[TEMP_CALIBRATE_STATIC_COUNT];
46 static short max_x, max_y, max_z;
47 static short min_x, min_y, min_z;
48 static char is_first = 1;
49 int i, delta_sum = 0;
50
51 count_static++;
52
53 if (is_first) {
54 temp_x[0] = x;
55 temp_y[0] = y;
56 temp_z[0] = z;
57 for (i = 1; i < TEMP_CALIBRATE_STATIC_COUNT; i++) {
58 temp_x[i] = temp_x[0];
59 temp_y[i] = temp_y[0];
60 temp_z[i] = temp_z[0];
61 }
62 is_first = 0;
63 } else {
64 max_x = min_x = temp_x[1];
65 max_y = min_y = temp_y[1];
66 max_z = min_z = temp_z[1];
67
68 for (i = 0; i < TEMP_CALIBRATE_STATIC_COUNT; i++) {
69 if (i == TEMP_CALIBRATE_STATIC_COUNT - 1) {
70 temp_x[i] = x;
71 temp_y[i] = y;
72 temp_z[i] = z;
73 } else {
74 temp_x[i] = temp_x[i+1];
75 temp_y[i] = temp_y[i+1];
76 temp_z[i] = temp_z[i+1];
77 }
78 max_x = (max_x > temp_x[i]) ? max_x:temp_x[i];
79 max_y = (max_y > temp_y[i]) ? max_y:temp_y[i];
80 max_z = (max_z > temp_z[i]) ? max_z:temp_z[i];
81
82 min_x = (min_x < temp_x[i]) ? min_x:temp_x[i];
83 min_y = (min_y < temp_y[i]) ? min_y:temp_y[i];
84 min_z = (min_z < temp_z[i]) ? min_z:temp_z[i];
85 }
86 }
87
88 if (count_static > TEMP_CALIBRATE_STATIC_COUNT) {
89 count_static = TEMP_CALIBRATE_STATIC_COUNT;
90 delta_sum = abs(max_x - min_x) + abs(max_y - min_y) +
91 abs(max_z - min_z);
92
93 if (delta_sum < TEMP_CALIBRATE_STATIC_THRESHOLD)
94 return 1;
95 }
96
97 return 0;
98 }
99
da228e_temp_calibrate(int * x,int * y,int * z)100 int da228e_temp_calibrate(int *x, int *y, int *z)
101 {
102 int tem_z = 0;
103 int cus = MIR3DA_OFFSET_MAX-MIR3DA_OFFSET_CUS;
104 int is_static = 0;
105 short lz_offset;
106
107 *z = *z + z_offset;
108 lz_offset = (*z) % 10;
109
110 if ((abs(*x) < MIR3DA_OFFSET_MAX) &&
111 (abs(*y) < MIR3DA_OFFSET_MAX)) {
112 is_static = da228e_temp_calibrate_detect_static(*x, *y, *z-z_offset);
113 tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN -
114 (*x)*(*x) - (*y)*(*y)) + lz_offset;
115 if (z_offset == 0) {
116 if (is_static == 1)
117 z_offset = (*z >= 0) ? (tem_z-*z) : (-tem_z-*z);
118 *z = ((*z >= 0) ? (1) : (-1))*tem_z;
119 } else if (is_static) {
120 if (abs(abs(*z) - MIR3DA_OFFSET_SEN) > MIR3DA_OFFSET_CUS) {
121 *z = ((*z >= 0) ? (1) : (-1))*tem_z;
122 z_offset = 0;
123 }
124 }
125 *x = (*x)*MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
126 *y = (*y)*MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
127
128 } else if ((abs((abs(*x) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
129 (abs(*y) < MIR3DA_OFFSET_MAX) && (z_offset)) {
130 if (abs(*x) > MIR3DA_OFFSET_SEN) {
131 *x = (*x > 0) ?
132 (*x - (abs(*x) - MIR3DA_OFFSET_SEN)*cus/MIR3DA_OFFSET_MAX) :
133 (*x + (abs(*x) - MIR3DA_OFFSET_SEN)*cus/MIR3DA_OFFSET_MAX);
134 } else {
135 *x = (*x > 0) ?
136 (*x + (MIR3DA_OFFSET_SEN - abs(*x))*cus/MIR3DA_OFFSET_MAX) :
137 (*x - (MIR3DA_OFFSET_SEN-abs(*x))*cus/MIR3DA_OFFSET_MAX);
138 }
139 *y = (*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
140 } else if ((abs((abs(*y) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
141 (abs(*x) < MIR3DA_OFFSET_MAX) && (z_offset)) {
142 if (abs(*y) > MIR3DA_OFFSET_SEN) {
143 *y = (*y > 0) ?
144 (*y - (abs(*y) - MIR3DA_OFFSET_SEN)*cus/MIR3DA_OFFSET_MAX) :
145 (*y + (abs(*y)-MIR3DA_OFFSET_SEN)*cus/MIR3DA_OFFSET_MAX);
146 } else {
147 *y = (*y > 0) ?
148 (*y + (MIR3DA_OFFSET_SEN - abs(*y))*cus/MIR3DA_OFFSET_MAX) :
149 (*y - (MIR3DA_OFFSET_SEN - abs(*y))*cus/MIR3DA_OFFSET_MAX);
150 }
151 *x = (*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
152 } else if (z_offset == 0) {
153 if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs((*y > 0) ?
154 (MIR3DA_OFFSET_SEN - *y) : (MIR3DA_OFFSET_SEN + *y)) < MIR3DA_OFFSET_MAX)) {
155 *z = ((*z >= 0) ?
156 (1) : (-1))*abs(*x)*MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
157 } else if ((abs(*y) < MIR3DA_OFFSET_MAX) &&
158 (abs((*x > 0) ? (MIR3DA_OFFSET_SEN - *x) : (MIR3DA_OFFSET_SEN + *x))
159 < MIR3DA_OFFSET_MAX)) {
160 *z = ((*z >= 0) ? (1) : (-1)) *
161 abs(*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
162 } else {
163 tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN -
164 (*x)*(*x) - (*y)*(*y)) + lz_offset;
165 *z = ((*z >= 0) ? (1) : (-1))*tem_z;
166 }
167 }
168
169 if (z_offset)
170 return 0;
171 else
172 return -1;
173 }
174
175
176