strat9_kernel/framebuffer/x86/
sse2.rs

1use core::arch::x86_64::*;
2
3#[inline]
4unsafe fn load_u32_unaligned(src: *const u8) -> i32 {
5    core::ptr::read_unaligned(src as *const u32) as i32
6}
7
8#[target_feature(enable = "sse2")]
9pub unsafe fn fill_sse2(dst: *mut u32, color: u32, count: usize) {
10    let color_vec = _mm_set1_epi32(color as i32);
11    let mut i = 0;
12
13    while i + 4 <= count {
14        _mm_storeu_si128(dst.add(i) as *mut __m128i, color_vec);
15        i += 4;
16    }
17
18    while i < count {
19        *dst.add(i) = color;
20        i += 1;
21    }
22}
23
24#[target_feature(enable = "sse2")]
25pub unsafe fn blit_sse2(dst: *mut u32, src: *const u32, count: usize) {
26    let mut i = 0;
27
28    while i + 4 <= count {
29        let src_vec = _mm_loadu_si128(src.add(i) as *const __m128i);
30        _mm_storeu_si128(dst.add(i) as *mut __m128i, src_vec);
31        i += 4;
32    }
33
34    while i < count {
35        *dst.add(i) = *src.add(i);
36        i += 1;
37    }
38}
39
40// In sse2, we use SSE4.1 blend where possible, but if only sse2 is available,
41// we'll implement a basic one. For simplicity, we use SSE2 compatible intrinsics.
42#[target_feature(enable = "sse2")]
43pub unsafe fn blend_sse2(dst: *mut u32, src: *const u32, alpha: u8, count: usize) {
44    // Basic SSE2 implementation for blend
45    let alpha_u16 = alpha as u16;
46    let inv_alpha = 255 - alpha_u16;
47
48    let alpha_vec = _mm_set1_epi16(alpha_u16 as i16);
49    let inv_alpha_vec = _mm_set1_epi16(inv_alpha as i16);
50    let zero = _mm_setzero_si128();
51    let ones = _mm_set1_epi16(1);
52
53    let mut i = 0;
54    while i + 4 <= count {
55        let d = _mm_loadu_si128(dst.add(i) as *const __m128i);
56        let s = _mm_loadu_si128(src.add(i) as *const __m128i);
57
58        let d_lo = _mm_unpacklo_epi8(d, zero);
59        let d_hi = _mm_unpackhi_epi8(d, zero);
60        let s_lo = _mm_unpacklo_epi8(s, zero);
61        let s_hi = _mm_unpackhi_epi8(s, zero);
62
63        let res_lo_s = _mm_mullo_epi16(s_lo, alpha_vec);
64        let res_lo_d = _mm_mullo_epi16(d_lo, inv_alpha_vec);
65        let res_lo = _mm_add_epi16(res_lo_s, res_lo_d);
66        // Exact /255: (x + (x>>8) + 1) >> 8  (dav1d formula)
67        let lo_div = _mm_srli_epi16(res_lo, 8);
68        let lo_corr = _mm_add_epi16(res_lo, _mm_add_epi16(lo_div, ones));
69        let res_lo_final = _mm_srli_epi16(lo_corr, 8);
70
71        let res_hi_s = _mm_mullo_epi16(s_hi, alpha_vec);
72        let res_hi_d = _mm_mullo_epi16(d_hi, inv_alpha_vec);
73        let res_hi = _mm_add_epi16(res_hi_s, res_hi_d);
74        let hi_div = _mm_srli_epi16(res_hi, 8);
75        let hi_corr = _mm_add_epi16(res_hi, _mm_add_epi16(hi_div, ones));
76        let res_hi_final = _mm_srli_epi16(hi_corr, 8);
77
78        let res = _mm_packus_epi16(res_lo_final, res_hi_final);
79        _mm_storeu_si128(dst.add(i) as *mut __m128i, res);
80        i += 4;
81    }
82
83    // fallback
84    if i < count {
85        crate::framebuffer::generic::blend_generic(dst.add(i), src.add(i), alpha, count - i);
86    }
87}
88
89#[target_feature(enable = "sse2")]
90pub unsafe fn convert_bgr_to_argb_sse2(dst: *mut u32, src: *const u8, count: usize) {
91    convert_bgr_to_argb_ssse3(dst, src, count);
92}
93
94/// BGR24 → ARGB32 conversion using SSSE3 pshufb (4 pixels/iter)
95#[target_feature(enable = "ssse3")]
96unsafe fn convert_bgr_to_argb_ssse3(dst: *mut u32, src: *const u8, count: usize) {
97    // Shuffle mask: rearranges BGR BGR BGR BGR → B G R 0 B G R 0 B G R 0 B G R 0
98    // Input bytes:  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15
99    //             [B0 G0 R0 B1 G1 R1 B2 G2 R2 B3 G3 R3  ?  ?  ?  ?]
100    // Output:      [B0 G0 R0  0 B1 G1 R1  0 B2 G2 R2  0 B3 G3 R3  0]
101    #[rustfmt::skip]
102    let shuffle_mask = _mm_set_epi8(
103        -1, 11, 10,  9,     // pixel 3: 0, R3, G3, B3
104        -1,  8,  7,  6,     // pixel 2: 0, R2, G2, B2
105        -1,  5,  4,  3,     // pixel 1: 0, R1, G1, B1
106        -1,  2,  1,  0,     // pixel 0: 0, R0, G0, B0
107    );
108    // Alpha mask: sets byte 3,7,11,15 to 0xFF
109    let alpha_mask = _mm_set1_epi32(0xFF000000_u32 as i32);
110
111    let mut i = 0;
112    // Charge exactement 12 octets (4 pixels BGR) sans dépassement de buffer
113    while i + 4 <= count {
114        // lecture sécurisée: 8 premiers octets (B0..R1) + 4 suivants (B2..R3)
115        let lo8 = _mm_loadl_epi64(src.add(i * 3) as *const __m128i);
116        let hi4 = _mm_cvtsi32_si128(load_u32_unaligned(src.add(i * 3 + 8)));
117        let raw = _mm_unpacklo_epi64(lo8, hi4);
118        let shuffled = _mm_shuffle_epi8(raw, shuffle_mask);
119        let result = _mm_or_si128(shuffled, alpha_mask);
120        _mm_storeu_si128(dst.add(i) as *mut __m128i, result);
121        i += 4;
122    }
123
124    // tail
125    if i < count {
126        crate::framebuffer::generic::convert_bgr_to_argb_generic(
127            dst.add(i),
128            src.add(i * 3),
129            count - i,
130        );
131    }
132}
strat9_kernel/framebuffer/x86/sse2.rs

strat9_kernel/framebuffer/x86/
sse2.rs
