aboutsummaryrefslogtreecommitdiff
path: root/nn.c
blob: 373c854f947e046a92dc08ffd9b0653219f4fea2 (plain) (blame)
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
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
#include "gemm.h"
#include "nn.h"

#include <math.h>
#include <string.h>

void facedet_nn_conv2d(const float *x,
                   int cin,
                   int ih,
                   int iw,
                   const float *w,
                   const float *bias,
                   int cout,
                   int k,
                   int stride,
                   int pad,
                   int group,
                   int relu,
                   float *y,
                   int oh,
                   int ow) {
    int cin_g = cin / group;
    int cout_g = cout / group;
    for (int oc = 0; oc < cout; oc++) {
        int g = oc / cout_g;
        const float *woc = w + (size_t)oc * cin_g * k * k;
        float *yc = y + (size_t)oc * oh * ow;
        for (int oy = 0; oy < oh; oy++) {
            for (int ox = 0; ox < ow; ox++) {
                float acc = bias[oc];
                int iy0 = oy * stride - pad;
                int ix0 = ox * stride - pad;
                for (int ic = 0; ic < cin_g; ic++) {
                    const float *xc = x + (size_t)(g * cin_g + ic) * ih * iw;
                    const float *wk = woc + (size_t)ic * k * k;
                    for (int ky = 0; ky < k; ky++) {
                        int iy = iy0 + ky;
                        if (iy < 0 || iy >= ih)
                            continue;
                        const float *row = xc + (size_t)iy * iw;
                        const float *wr = wk + (size_t)ky * k;
                        for (int kx = 0; kx < k; kx++) {
                            int ix = ix0 + kx;
                            if (ix < 0 || ix >= iw)
                                continue;
                            acc += row[ix] * wr[kx];
                        }
                    }
                }
                yc[(size_t)oy * ow + ox] = relu && acc < 0.0f ? 0.0f : acc;
            }
        }
    }
}

void facedet_nn_im2col3(const float *x, int cin, int ih, int iw, int stride, float *col, int oh, int ow) {
    size_t n = (size_t)oh * ow;
    for (int ic = 0; ic < cin; ic++) {
        const float *xc = x + (size_t)ic * ih * iw;
        for (int ky = 0; ky < 3; ky++) {
            for (int kx = 0; kx < 3; kx++) {
                float *dst = col + ((size_t)(ic * 9 + ky * 3 + kx)) * n;
                for (int oy = 0; oy < oh; oy++) {
                    int iy = oy * stride - 1 + ky;
                    float *d = dst + (size_t)oy * ow;
                    if (iy < 0 || iy >= ih) {
                        memset(d, 0, sizeof(float) * ow);
                        continue;
                    }
                    const float *row = xc + (size_t)iy * iw;
                    if (stride == 1) {
                        int ox = 0;
                        if (kx == 0) {
                            d[0] = 0.0f;
                            ox = 1;
                        }
                        int last = ow;
                        if (kx == 2)
                            last = ow - 1;
                        if (last > ox)
                            memcpy(d + ox, row + ox - 1 + kx, sizeof(float) * (size_t)(last - ox));
                        if (kx == 2)
                            d[ow - 1] = 0.0f;
                    } else {
                        for (int ox = 0; ox < ow; ox++) {
                            int ix = ox * stride - 1 + kx;
                            d[ox] = (ix < 0 || ix >= iw) ? 0.0f : row[ix];
                        }
                    }
                }
            }
        }
    }
}

#if defined(__aarch64__) && !defined(FACEDET_SCALAR)
#    include <arm_neon.h>
void facedet_nn_dwconv3(
    const float *x, int c, int ih, int iw, const float *w, const float *bias, int relu, float *y) {
    for (int ch = 0; ch < c; ch++) {
        const float *xc = x + (size_t)ch * ih * iw;
        const float *wc = w + (size_t)ch * 9;
        float *yc = y + (size_t)ch * ih * iw;
        float b = bias[ch];
        float32x4_t wk[9];
        for (int i = 0; i < 9; i++)
            wk[i] = vdupq_n_f32(wc[i]);
        for (int oy = 0; oy < ih; oy++) {
            const float *pm = oy > 0 ? xc + (size_t)(oy - 1) * iw : NULL;
            const float *pc = xc + (size_t)oy * iw;
            const float *pp = oy < ih - 1 ? xc + (size_t)(oy + 1) * iw : NULL;
            float *out = yc + (size_t)oy * iw;
            int x1 = iw - 1;
            const int edges[2] = {0, x1};
            for (int e = 0; e < 2; e++) {
                int ex = edges[e];
                float acc = b;
                for (int ky = 0; ky < 3; ky++) {
                    const float *row = ky == 0 ? pm : ky == 1 ? pc : pp;
                    if (!row)
                        continue;
                    for (int kx = 0; kx < 3; kx++) {
                        int ix = ex - 1 + kx;
                        if (ix < 0 || ix >= iw)
                            continue;
                        acc += wc[ky * 3 + kx] * row[ix];
                    }
                }
                out[ex] = relu && acc < 0.0f ? 0.0f : acc;
            }
            int ox = 1;
            for (; ox + 4 <= x1; ox += 4) {
                float32x4_t acc = vdupq_n_f32(b);
                if (pm) {
                    acc = vfmaq_f32(acc, wk[0], vld1q_f32(pm + ox - 1));
                    acc = vfmaq_f32(acc, wk[1], vld1q_f32(pm + ox));
                    acc = vfmaq_f32(acc, wk[2], vld1q_f32(pm + ox + 1));
                }
                acc = vfmaq_f32(acc, wk[3], vld1q_f32(pc + ox - 1));
                acc = vfmaq_f32(acc, wk[4], vld1q_f32(pc + ox));
                acc = vfmaq_f32(acc, wk[5], vld1q_f32(pc + ox + 1));
                if (pp) {
                    acc = vfmaq_f32(acc, wk[6], vld1q_f32(pp + ox - 1));
                    acc = vfmaq_f32(acc, wk[7], vld1q_f32(pp + ox));
                    acc = vfmaq_f32(acc, wk[8], vld1q_f32(pp + ox + 1));
                }
                if (relu)
                    acc = vmaxq_f32(acc, vdupq_n_f32(0.0f));
                vst1q_f32(out + ox, acc);
            }
            for (; ox < x1; ox++) {
                float acc = b;
                for (int ky = 0; ky < 3; ky++) {
                    const float *row = ky == 0 ? pm : ky == 1 ? pc : pp;
                    if (!row)
                        continue;
                    for (int kx = 0; kx < 3; kx++)
                        acc += wc[ky * 3 + kx] * row[ox - 1 + kx];
                }
                out[ox] = relu && acc < 0.0f ? 0.0f : acc;
            }
        }
    }
}
#else
void facedet_nn_dwconv3(
    const float *x, int c, int ih, int iw, const float *w, const float *bias, int relu, float *y) {
    facedet_nn_conv2d(x, c, ih, iw, w, bias, c, 3, 1, 1, c, relu, y, ih, iw);
}
#endif

void facedet_nn_maxpool2(const float *x, int c, int ih, int iw, float *y) {
    int oh = ih / 2, ow = iw / 2;
    for (int ch = 0; ch < c; ch++) {
        const float *xc = x + (size_t)ch * ih * iw;
        float *yc = y + (size_t)ch * oh * ow;
        for (int oy = 0; oy < oh; oy++)
            for (int ox = 0; ox < ow; ox++) {
                const float *p = xc + (size_t)(oy * 2) * iw + ox * 2;
                float a = p[0] > p[1] ? p[0] : p[1];
                float b = p[iw] > p[iw + 1] ? p[iw] : p[iw + 1];
                yc[(size_t)oy * ow + ox] = a > b ? a : b;
            }
    }
}

void facedet_nn_resize2x(const float *x, int c, int ih, int iw, float *y) {
    int oh = ih * 2, ow = iw * 2;
    for (int ch = 0; ch < c; ch++) {
        const float *xc = x + (size_t)ch * ih * iw;
        float *yc = y + (size_t)ch * oh * ow;
        for (int oy = 0; oy < oh; oy++) {
            const float *row = xc + (size_t)(oy / 2) * iw;
            float *out = yc + (size_t)oy * ow;
            for (int ox = 0; ox < ow; ox++)
                out[ox] = row[ox / 2];
        }
    }
}

void facedet_nn_add(const float *a, const float *b, float *y, size_t n) {
    for (size_t i = 0; i < n; i++)
        y[i] = a[i] + b[i];
}

float facedet_nn_sigmoid(float v) { return 1.0f / (1.0f + expf(-v)); }