-
Notifications
You must be signed in to change notification settings - Fork 0
/
SKP_Silk_find_LTP_FIX.go
172 lines (169 loc) · 6.62 KB
/
SKP_Silk_find_LTP_FIX.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
package silk
import "unsafe"
const LTP_CORRS_HEAD_ROOM = 2
func SKP_Silk_find_LTP_FIX(b_Q14 [20]int16, WLTP [100]int32, LTPredCodGain_Q7 *int32, r_first []int16, r_last []int16, lag [4]int32, Wght_Q15 [4]int32, subfr_length int32, mem_offset int32, corr_rshifts [4]int32) {
var (
i int32
k int32
lshift int32
r_ptr []int16
lag_ptr *int16
b_Q14_ptr *int16
regu int32
WLTP_ptr []int32
b_Q16 [5]int32
delta_b_Q14 [5]int32
d_Q14 [4]int32
nrg [4]int32
g_Q26 int32
w [4]int32
WLTP_max int32
max_abs_d_Q14 int32
max_w_bits int32
temp32 int32
denom32 int32
extra_shifts int32
rr_shifts int32
maxRshifts int32
maxRshifts_wxtra int32
LZs int32
LPC_res_nrg int32
LPC_LTP_res_nrg int32
div_Q16 int32
Rr [5]int32
rr [4]int32
wd int32
m_Q12 int32
)
b_Q14_ptr = &b_Q14[0]
WLTP_ptr = ([]int32)(WLTP[:])
r_ptr = ([]int16)(&r_first[mem_offset])
for k = 0; k < NB_SUBFR; k++ {
if k == (NB_SUBFR >> 1) {
r_ptr = ([]int16)(&r_last[mem_offset])
}
lag_ptr = (*int16)(unsafe.Add(unsafe.Pointer(&r_ptr[0]), -int(unsafe.Sizeof(int16(0))*uintptr(lag[k]+LTP_ORDER/2))))
SKP_Silk_sum_sqr_shift(&rr[k], &rr_shifts, r_ptr, subfr_length)
LZs = SKP_Silk_CLZ32(rr[k])
if LZs < LTP_CORRS_HEAD_ROOM {
rr[k] = SKP_RSHIFT_ROUND(rr[k], LTP_CORRS_HEAD_ROOM-LZs)
rr_shifts += LTP_CORRS_HEAD_ROOM - LZs
}
corr_rshifts[k] = rr_shifts
SKP_Silk_corrMatrix_FIX(([]int16)(lag_ptr), subfr_length, LTP_ORDER, LTP_CORRS_HEAD_ROOM, &WLTP_ptr[0], &corr_rshifts[k])
SKP_Silk_corrVector_FIX(([]int16)(lag_ptr), r_ptr, subfr_length, LTP_ORDER, Rr[:], corr_rshifts[k])
if corr_rshifts[k] > rr_shifts {
rr[k] = (rr[k]) >> (corr_rshifts[k] - rr_shifts)
}
SKP_assert(rr[k] >= 0)
regu = 1
regu = SKP_SMLAWB(regu, rr[k], SKP_FIX_CONST(0.01/3, 16))
regu = SKP_SMLAWB(regu, WLTP_ptr[LTP_ORDER*0+0], SKP_FIX_CONST(0.01/3, 16))
regu = SKP_SMLAWB(regu, WLTP_ptr[(LTP_ORDER-1)*LTP_ORDER+(LTP_ORDER-1)], SKP_FIX_CONST(0.01/3, 16))
SKP_Silk_regularize_correlations_FIX(WLTP_ptr, ([]int32)(&rr[k]), regu, LTP_ORDER)
SKP_Silk_solve_LDL_FIX(&WLTP_ptr[0], LTP_ORDER, &Rr[0], &b_Q16[0])
SKP_Silk_fit_LTP(b_Q16[:], ([]int16)(b_Q14_ptr))
nrg[k] = SKP_Silk_residual_energy16_covar_FIX(b_Q14_ptr, &WLTP_ptr[0], &Rr[0], rr[k], LTP_ORDER, 14)
extra_shifts = SKP_min_int(corr_rshifts[k], LTP_CORRS_HEAD_ROOM)
denom32 = SKP_LSHIFT_SAT32(SKP_SMULWB(nrg[k], Wght_Q15[k]), extra_shifts+1) + (SKP_SMULWB(subfr_length, 655) >> (corr_rshifts[k] - extra_shifts))
if denom32 > 1 {
denom32 = denom32
} else {
denom32 = 1
}
SKP_assert((int64(Wght_Q15[k]) << 16) < SKP_int32_MAX)
temp32 = ((Wght_Q15[k]) << 16) / denom32
temp32 = temp32 >> (corr_rshifts[k] + 31 - extra_shifts - 26)
WLTP_max = 0
for i = 0; i < LTP_ORDER*LTP_ORDER; i++ {
if (WLTP_ptr[i]) > WLTP_max {
WLTP_max = WLTP_ptr[i]
} else {
WLTP_max = WLTP_max
}
}
lshift = SKP_Silk_CLZ32(WLTP_max) - 1 - 3
SKP_assert(lshift+(26-18) >= 0)
if lshift+(26-18) < 31 {
temp32 = SKP_min_32(temp32, 1<<(lshift+(26-18)))
}
SKP_Silk_scale_vector32_Q26_lshift_18(WLTP_ptr, temp32, LTP_ORDER*LTP_ORDER)
w[k] = WLTP_ptr[(LTP_ORDER>>1)*LTP_ORDER+(LTP_ORDER>>1)]
SKP_assert(w[k] >= 0)
r_ptr += ([]int16)(subfr_length)
b_Q14_ptr = (*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(LTP_ORDER)))
WLTP_ptr += LTP_ORDER * LTP_ORDER
}
maxRshifts = 0
for k = 0; k < NB_SUBFR; k++ {
maxRshifts = SKP_max_int(corr_rshifts[k], maxRshifts)
}
if LTPredCodGain_Q7 != nil {
LPC_LTP_res_nrg = 0
LPC_res_nrg = 0
SKP_assert(LTP_CORRS_HEAD_ROOM >= 2)
for k = 0; k < NB_SUBFR; k++ {
LPC_res_nrg = LPC_res_nrg + ((SKP_SMULWB(rr[k], Wght_Q15[k]) + 1) >> ((maxRshifts - corr_rshifts[k]) + 1))
LPC_LTP_res_nrg = LPC_LTP_res_nrg + ((SKP_SMULWB(nrg[k], Wght_Q15[k]) + 1) >> ((maxRshifts - corr_rshifts[k]) + 1))
}
if LPC_LTP_res_nrg > 1 {
LPC_LTP_res_nrg = LPC_LTP_res_nrg
} else {
LPC_LTP_res_nrg = 1
}
div_Q16 = SKP_DIV32_varQ(LPC_res_nrg, LPC_LTP_res_nrg, 16)
*LTPredCodGain_Q7 = SKP_SMULBB(3, SKP_Silk_lin2log(div_Q16)-(16<<7))
SKP_assert(*LTPredCodGain_Q7 == int32(SKP_SAT16((SKP_Silk_lin2log(div_Q16)-(16<<7))*3)))
}
b_Q14_ptr = &b_Q14[0]
for k = 0; k < NB_SUBFR; k++ {
d_Q14[k] = 0
for i = 0; i < LTP_ORDER; i++ {
d_Q14[k] += int32(*(*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(i))))
}
b_Q14_ptr = (*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(LTP_ORDER)))
}
max_abs_d_Q14 = 0
max_w_bits = 0
for k = 0; k < NB_SUBFR; k++ {
max_abs_d_Q14 = SKP_max_32(max_abs_d_Q14, int32(SKP_abs(int64(d_Q14[k]))))
max_w_bits = SKP_max_32(max_w_bits, 32-SKP_Silk_CLZ32(w[k])+corr_rshifts[k]-maxRshifts)
}
SKP_assert(max_abs_d_Q14 <= (5 << 15))
extra_shifts = max_w_bits + 32 - SKP_Silk_CLZ32(max_abs_d_Q14) - 14
extra_shifts -= maxRshifts + (32 - 1 - 2)
extra_shifts = SKP_max_int(extra_shifts, 0)
maxRshifts_wxtra = maxRshifts + extra_shifts
temp32 = (262 >> (maxRshifts + extra_shifts)) + 1
wd = 0
for k = 0; k < NB_SUBFR; k++ {
temp32 = temp32 + ((w[k]) >> (maxRshifts_wxtra - corr_rshifts[k]))
wd = wd + (SKP_SMULWW((w[k])>>(maxRshifts_wxtra-corr_rshifts[k]), d_Q14[k]) << 2)
}
m_Q12 = SKP_DIV32_varQ(wd, temp32, 12)
b_Q14_ptr = &b_Q14[0]
for k = 0; k < NB_SUBFR; k++ {
if 2-corr_rshifts[k] > 0 {
temp32 = (w[k]) >> (2 - corr_rshifts[k])
} else {
temp32 = SKP_LSHIFT_SAT32(w[k], corr_rshifts[k]-2)
}
g_Q26 = (SKP_FIX_CONST(0.1, 26) / ((SKP_FIX_CONST(0.1, 26) >> 10) + temp32)) * SKP_LSHIFT_SAT32(SKP_SUB_SAT32(m_Q12, (d_Q14[k])>>2), 4)
temp32 = 0
for i = 0; i < LTP_ORDER; i++ {
delta_b_Q14[i] = int32(SKP_max_16(*(*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(i))), 1638))
temp32 += delta_b_Q14[i]
}
temp32 = g_Q26 / temp32
for i = 0; i < LTP_ORDER; i++ {
*(*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(i))) = int16(SKP_LIMIT_32(int32(*(*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(i))))+SKP_SMULWB(SKP_LSHIFT_SAT32(temp32, 4), delta_b_Q14[i]), -16000, 28000))
}
b_Q14_ptr = (*int16)(unsafe.Add(unsafe.Pointer(b_Q14_ptr), unsafe.Sizeof(int16(0))*uintptr(LTP_ORDER)))
}
}
func SKP_Silk_fit_LTP(LTP_coefs_Q16 []int32, LTP_coefs_Q14 []int16) {
var i int32
for i = 0; i < LTP_ORDER; i++ {
LTP_coefs_Q14[i] = SKP_SAT16(SKP_RSHIFT_ROUND(LTP_coefs_Q16[i], 2))
}
}