Помогите написать программу С++

[С+-+]

Помогите написать программу С++

Составить программу, выполняющую операцию "сдвиг вправо". На входах
РА0...РА7 задается 8-ми разрядное двоичное число, на входах РВ0...РВ2
количество разрядов, на которое число сдвигается. Результат выводится непрерывно на выходах РD0...PD7.

есть только вот это
//ICC-AVR application builder : 22.05.2013 21:38:25
// Target : M8535
// Crystal: 8.0000Mhz

#include <iom8535v.h>
#include <macros.h>

void port_init(void)
{
PORTA = 0xFF;
PORTB = 0x07;
DDRD = 0xFF;
}


void main(void)
{
port_init();

while(1)
{
if((PINB&0x01)!=0)
{
PORTD=PORTA>>1;
}
if((PINB&0x02)!=0)
{
PORTD=PORTA>>2;
}
if((PINB&0x04)!=0)
{
PORTD=PORTA>>3;
}
}
}