When you say it doesn't work right, does it work backwards-- i.e., it sustains when up, and doesn't sustain when you press down on it? If so, it sounds like it's a problem with the polarity. You can start by doing the following:
1) Turn off the keyboard.
2) Plug the sustain pedal into the keyboard.
3) Make sure the pedal is up.
4) Turn on the keyboard.
5) See if it works correctly.
You should not plug the pedal in while the keyboard is already turned on, or turn on the keyboard while the pedal is depressed, as that will confuse the keyboard about the polarity.
If that doesn't work, check to see if the pedal itself has some kind of switch for setting the polarity. Yamaha pedals do not have polarity switches, but other brands often do.