Blend RGB555 colors together with "software packed" method

blendRGB555.c

#include <stdint.h>

uint16_t blendRGB555( uint16_t color1, uint16_t color2 ) {
    return( (color1 & color2) + (((color1 ^ color2) & 0x7bde) >> 1) );
}

// testbench

#include <stdio.h>
#define R_bits  5
#define G_bits  5
#define B_bits  5

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 = ( r1 + r2 ) / 2;
                            uint16_t g_expected = ( g1 + g2 ) / 2;
                            uint16_t b_expected = ( b1 + b2 ) / 2;

                            uint16_t res = blendRGB555( 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;
}