Fix MacroAssembler::SafepointRegisterStackIndex

to handle sparse kSafepointSavedRegisters bitmap.