ich würde mir an deiner Stelle für deine Software fertige Codebeispiele oder fertige libs für die mcp23017 suchen und dann darauf aufbauen.
Ist übrigens genau der Grund, weshalb ich zum Programmieren grundsätzlich Arduino mit fertigen C++ libs nutze.
Ich bin mir auch einigermaßen sicher, dass sich dein ATMega128 per Arduino programmieren lässt.