>    #define testbit(data,bitno) ((data>>bitno)&0x01)

If you change this to:

        #define testbit(data,bitno) (data&(1<<bitno))

then the compiler will have a much better chance of being able to
optimize it...

It's not wrong, just potentially slow...

BillW