Нужна помощь direct pin manipulation (управлять пинами через регистры)

[Junkie]

Здравствуйте. Нужно ускорение функций управления портом, стандартные digitalRead/Write, pinMode медленные из-за всяких проверок. Для ардуины я сделал такую замену все стандартным функциям (управление портом D через регистры)

int pins[8] = {PD0, PD1, PD2, PD3, PD4, PD5, PD6, PD7};

void dw(int pin, int st){ //digital write
  if(st==LOW) PORTD &= ~_BV(pins[pin]);
  if(st==HIGH) PORTD |= _BV(pins[pin]);
}

int dr(int pin){ //digital read
  if((PIND & (1<<pins[pin])) == 64) return HIGH;
  if((PIND & (1<<pins[pin])) == 0) return LOW;
}

void pm(int pin, int st){ //pinmode
  if(st==OUTPUT) DDRD |= (1<<pins[pin]);
  if(st==INPUT)  DDRD &= ~(1<<pins[pin]);
}

Теперь нужно перенести код на ESP. нашел тему про это
Direct GPIO port register manipulation - Everything ESP8266
и еще файл с директивами
ESP8266: esp8266/cores/esp8266/esp8266_peri.h Source File

Запись в порт получилось сделать, а как чтение и pinmode не знаю, может кто подскажет?

void dgitalWrite(int pin, int st){
  if(st==HIGH) GPOS = 1<<pin;
  if(st==LOW ) GPOC = 1<<pin;
}

 

[Junkie]

получилось все тоже самое

void dgitalWrite(int pin, int st){
  if(st==LOW ) GPO &= ~(1<<pin);
  if(st==HIGH) GPO |=  (1<<pin);
}

int dgitalRead(int pin){
  if((GPI & (1<<pin)) == 64) return HIGH;
  if((GPI & (1<<pin)) == 0) return LOW;
}

void pnMode(int pin, int st){
  if(st==INPUT ) GPE &= ~(1<<pin);
  if(st==OUTPUT) GPE |=  (1<<pin);
}

теперь другой интересный вопрос, после переключения ноги с записи на чтение, мне нужно вставлять задержку в 4 мкс перед считыванием, иначе получаются некорректные значения (выдает то что я записывал)