Add RGB565 colors together with "software packed" method

addRGB565.c

#include <stdint.h>

uint16_t addRGB565( uint16_t color1, uint16_t color2 ) {
    uint16_t c;
    c = ( ( color1 & color2 ) + ( ( ( color1 ^ color2 ) & 0xf7de ) >> 1)) & 0x8410;
    c = ( ( ( ( ( c + 0xfbdf0 ) ) >> 5 ) & 0xfbff ) + 0x200 ) ^ 0x7bef;

    return (color1 + color2 - c) | c;
}

// testbench

#include <stdio.h>
#define R_bits  5
#define G_bits  6
#define B_bits  5
#define MIN(a, b) ((a) < (b) ? (a) : (b))

int main(void) {
    for ( uint16_t r1 = 0 ; r1 < ( 1 << R_bits ) ; r1 ++ ) {
        printf( "%d\n", r1 );
        for ( uint16_t g1 = 0 ; g1 < ( 1 << G_bits ) ; g1 ++ ) {
            for ( uint16_t b1 = 0 ; b1 < ( 1 << B_bits ) ; b1 ++ ) {
                uint16_t color1 = ( r1 << ( G_bits + B_bits ) ) | ( g1 << B_bits ) | b1;
                for ( uint16_t r2 = 0 ; r2 < ( 1 << R_bits ) ; r2 ++ ) {
                    for ( uint16_t g2 = 0 ; g2 < ( 1 << G_bits ) ; g2 ++ ) {
                        for ( uint16_t b2 = 0 ; b2 < ( 1 << B_bits ) ; b2 ++ ) {
                            uint16_t color2 = ( r2 << ( G_bits + B_bits ) ) | ( g2 << B_bits ) | b2;
                            uint16_t r_expected = MIN( r1 + r2, ( 1 << R_bits ) - 1 );
                            uint16_t g_expected = MIN( g1 + g2, ( 1 << G_bits ) - 1 );
                            uint16_t b_expected = MIN( b1 + b2, ( 1 << B_bits ) - 1 );

                            uint16_t res = addRGB565( color1, color2 );

                            uint16_t r_result = ( res >> ( G_bits + B_bits ) ) & ( ( 1 << R_bits ) - 1 );
                            uint16_t g_result = ( res >>  B_bits ) & ( ( 1 << G_bits ) - 1 );
                            uint16_t b_result = res & ( ( 1 << B_bits ) - 1 );

                            if ( r_result != r_expected || g_result != g_expected || b_result != b_expected ) {
                                printf( "\nWrong Answer: (%d,%d,%d) (%d,%d,%d) -> (%d,%d,%d) but expected (%d,%d,%d)\n",
                                    r1, g1, b1,
                                    r2, g2, b2,
                                    r_result, g_result, b_result,
                                    r_expected, g_expected, b_expected
                                );
                                return 1;
                            }
                        }
                    }
                }
            }
        }
    }
    printf( "Succeeded!\n" );
    return 0;
}