Broken attempt to get OV7670 working with ESP32

esp_ov7670.ino

#include "sccb.h"
#include <driver/ledc.h>

#define PIN_XCLK 18  // Clock provided to camera
#define PIN_RESET 5  // Reset pin.

#define OV7670_ADDR (0x21)
#define OV7670_REG_PID    (0x0A)
#define OV7670_REG_VER    (0x0B)
#define OV7670_REG_MIDH   (0x1c)
#define OV7670_REG_MIDL   (0x1d)


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("\n\nOV7670 TEST\nStarting...");
  printf("Enable clock on pin %i\n", PIN_XCLK);
  enable_clock();
  printf("Start SCCB  SDA: %i SCL: %i\n", SDA, SCL);
  SCCB_Init(SDA, SCL);
  pinMode(PIN_RESET, OUTPUT);
  gpio_pulldown_en(GPIO_NUM_5);
  digitalWrite(PIN_RESET, LOW);
  delay(10);
  gpio_pulldown_dis(GPIO_NUM_5);
  digitalWrite(PIN_RESET, HIGH);
  delay(100);

  Serial.println("Scanning SCCB... ");
  Serial.println(SCCB_Probe(), HEX);
  delay(100);
  Serial.println(SCCB_Probe(), HEX);
  delay(100);
  Serial.println(SCCB_Probe(), HEX);
}

void loop() {
  // put your main code here, to run repeatedly:
  printf(
    "PID: %i VER %i\n",
    SCCB_Read(OV7670_ADDR, OV7670_REG_PID),
    SCCB_Read(OV7670_ADDR, OV7670_REG_VER)
  );
  delay(500);
}

int enable_clock() {
  periph_module_enable(PERIPH_LEDC_MODULE);

  ledc_timer_config_t timer_conf;
  timer_conf.bit_num    = (ledc_timer_bit_t)1;
  timer_conf.freq_hz    = 10000000;  // 10MHz
  timer_conf.speed_mode = LEDC_HIGH_SPEED_MODE;
  timer_conf.timer_num  = LEDC_TIMER_0;
  esp_err_t err = ledc_timer_config(&timer_conf);
  if (err != ESP_OK) {
      Serial.print("ledc_timer_config failed, rc=");
      Serial.println(err);
      return err;
  }

  ledc_channel_config_t ch_conf;
  ch_conf.channel    = LEDC_CHANNEL_0;
  ch_conf.timer_sel  = LEDC_TIMER_0;
  ch_conf.intr_type  = LEDC_INTR_DISABLE;
  ch_conf.duty       = 1;
  ch_conf.speed_mode = LEDC_HIGH_SPEED_MODE;
  ch_conf.gpio_num   = PIN_XCLK;

  err = ledc_channel_config(&ch_conf);
  if (err != ESP_OK) {
      Serial.print("ledc_channel_config failed, rc=");
      Serial.println(err);
      return err;
  }
  Serial.println("Clock enabled");
}

sccb.c

/*
 * This file is part of the OpenMV project.
 * Copyright (c) 2013/2014 Ibrahim Abdelkader <i.abdalkader@gmail.com>
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * SCCB (I2C like) driver.
 *
 */
#include <stdbool.h>
#include <Arduino.h>
#include "sccb.h"
#include "twi.h"
#include <stdio.h>

#define SCCB_FREQ   (100000) // We don't need fast I2C. 100KHz is fine here.
#define TIMEOUT     (1000) /* Can't be sure when I2C routines return. Interrupts
while polling hardware may result in unknown delays. */


int SCCB_Init(int pin_sda, int pin_scl)
{
    twi_init(pin_sda, pin_scl);
    return 0;
}

uint8_t SCCB_Probe()
{
    uint8_t reg = 0x00;
    uint8_t slv_addr = 0x00;

    for (uint8_t i=0; i<127; i++) {
        if (twi_writeTo(i, &reg, 1, true) == 0) {
            slv_addr = i;
            break;
        }

        if (i!=126) {
            delay(1); // Necessary for OV7725 camera (not for OV2640).
        }
    }
    return slv_addr;
}

uint8_t SCCB_Read(uint8_t slv_addr, uint8_t reg)
{
    uint8_t data=0;

    noInterrupts();
    int rc = twi_writeTo(slv_addr, &reg, 1, true);
    if (rc != 0) {
        data = 0xff;
    }
    else {
        rc = twi_readFrom(slv_addr, &data, 1, true);
        if (rc != 0) {
            data=0xFF;
        }
    }
    interrupts();
    if (rc != 0) {
        printf("SCCB_Read [%02x] failed rc=%d\n", reg, rc);
    }
    return data;
}

uint8_t SCCB_Write(uint8_t slv_addr, uint8_t reg, uint8_t data)
{
    uint8_t ret=0;
    uint8_t buf[] = {reg, data};

    noInterrupts();
    if(twi_writeTo(slv_addr, buf, 2, true) != 0) {
        ret=0xFF;
    }
    interrupts();
    if (ret != 0) {
        printf("SCCB_Write [%02x]=%02x failed\n", reg, data);
    }
    return ret;
}

sccb.h

/*
 * This file is part of the OpenMV project.
 * Copyright (c) 2013/2014 Ibrahim Abdelkader <i.abdalkader@gmail.com>
 * This work is licensed under the MIT license, see the file LICENSE for details.
 *
 * SCCB (I2C like) driver.
 *
 */
#ifndef __SCCB_H__
#define __SCCB_H__

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
int SCCB_Init(int pin_sda, int pin_scl);
uint8_t SCCB_Probe();
uint8_t SCCB_Read(uint8_t slv_addr, uint8_t reg);
uint8_t SCCB_Write(uint8_t slv_addr, uint8_t reg, uint8_t data);

#ifdef __cplusplus
}
#endif

#endif // __SCCB_H__

twi.c

/*
  si2c.c - Software I2C library for ESP31B

  Copyright (c) 2015 Hristo Gochkov. All rights reserved.
  This file is part of the ESP31B core for Arduino environment.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/
#include <stdint.h>
#include <stdbool.h>
#include "twi.h"
#include "soc/gpio_reg.h"
#include <esp32-hal-gpio.h>
#include <stdio.h>

unsigned char twi_dcount = 18;
static unsigned char twi_sda, twi_scl;


static inline void SDA_LOW() {
  //Enable SDA (becomes output and since GPO is 0 for the pin, 
  // it will pull the line low)
  if (twi_sda < 32) {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, BIT(twi_sda));
  }
  else {
    REG_WRITE(GPIO_ENABLE1_W1TS_REG, BIT(twi_sda - 32));
  }
}

static inline void SDA_HIGH() {
  //Disable SDA (becomes input and since it has pullup it will go high)
  if (twi_sda < 32) {
    REG_WRITE(GPIO_ENABLE_W1TC_REG, BIT(twi_sda));
  }
  else {
    REG_WRITE(GPIO_ENABLE1_W1TC_REG, BIT(twi_sda - 32));
  }
}

static inline uint32_t SDA_READ() { 
  if (twi_sda < 32) {
    return (REG_READ(GPIO_IN_REG) & BIT(twi_sda)) != 0;
  }
  else {
    return (REG_READ(GPIO_IN1_REG) & BIT(twi_sda - 32)) != 0;
  }
}

static void SCL_LOW() {
  if (twi_scl < 32) {
    REG_WRITE(GPIO_ENABLE_W1TS_REG, BIT(twi_scl));
  }
  else {
    REG_WRITE(GPIO_ENABLE1_W1TS_REG, BIT(twi_scl - 32));
  }
}

static void SCL_HIGH() {
  if (twi_scl < 32) {
    REG_WRITE(GPIO_ENABLE_W1TC_REG, BIT(twi_scl));
  }
  else {
    REG_WRITE(GPIO_ENABLE1_W1TC_REG, BIT(twi_scl - 32));
  }
}

static uint32_t SCL_READ() {
  if (twi_scl < 32) {
    return (REG_READ(GPIO_IN_REG) & BIT(twi_scl)) != 0;
  }
  else {
    return (REG_READ(GPIO_IN1_REG) & BIT(twi_scl - 32)) != 0;
  }
}


#ifndef FCPU80
#define FCPU80 80000000L
#endif

#if F_CPU == FCPU80
#define TWI_CLOCK_STRETCH 800
#else
#define TWI_CLOCK_STRETCH 1600
#endif

void twi_setClock(unsigned int freq){
#if F_CPU == FCPU80
  if(freq <= 100000) twi_dcount = 19;//about 100KHz
  else if(freq <= 200000) twi_dcount = 8;//about 200KHz
  else if(freq <= 300000) twi_dcount = 3;//about 300KHz
  else if(freq <= 400000) twi_dcount = 1;//about 400KHz
  else twi_dcount = 1;//about 400KHz
#else
  if(freq <= 100000) twi_dcount = 32;//about 100KHz
  else if(freq <= 200000) twi_dcount = 14;//about 200KHz
  else if(freq <= 300000) twi_dcount = 8;//about 300KHz
  else if(freq <= 400000) twi_dcount = 5;//about 400KHz
  else if(freq <= 500000) twi_dcount = 3;//about 500KHz
  else if(freq <= 600000) twi_dcount = 2;//about 600KHz
  else twi_dcount = 1;//about 700KHz
#endif
}

void twi_init(unsigned char sda, unsigned char scl){
  twi_sda = sda;
  twi_scl = scl;
  pinMode(twi_sda, OUTPUT);
  pinMode(twi_scl, OUTPUT);

  digitalWrite(twi_sda, 0);
  digitalWrite(twi_scl, 0);

  pinMode(twi_sda, INPUT_PULLUP);
  pinMode(twi_scl, INPUT_PULLUP);
  twi_setClock(100000);
}

void twi_stop(void){
  pinMode(twi_sda, INPUT);
  pinMode(twi_scl, INPUT);
}

static void twi_delay(unsigned char v){
  unsigned int i;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  unsigned int reg;
  for(i=0;i<v;i++) reg = REG_READ(GPIO_IN_REG);
#pragma GCC diagnostic pop
}

static bool twi_write_start(void) {
  SCL_HIGH();
  SDA_HIGH();
  if (SDA_READ() == 0) return false;
  twi_delay(twi_dcount);
  SDA_LOW();
  twi_delay(twi_dcount);
  return true;
}

static bool twi_write_stop(void){
  unsigned int i = 0;
  SCL_LOW();
  SDA_LOW();
  twi_delay(twi_dcount);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH);// Clock stretching (up to 100us)
  twi_delay(twi_dcount);
  SDA_HIGH();
  twi_delay(twi_dcount);

  return true;
}

bool do_log = false;
static bool twi_write_bit(bool bit) {
  unsigned int i = 0;
  SCL_LOW();
  if (bit) {SDA_HIGH(); if (do_log) {twi_delay(twi_dcount+1);}}
  else {SDA_LOW(); if (do_log) {} }
  twi_delay(twi_dcount+1);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH);// Clock stretching (up to 100us)
  twi_delay(twi_dcount);
  return true;
}

static bool twi_read_bit(void) {
  unsigned int i = 0;
  SCL_LOW();
  SDA_HIGH();
  twi_delay(twi_dcount+2);
  SCL_HIGH();
  while (SCL_READ() == 0 && (i++) < TWI_CLOCK_STRETCH);// Clock stretching (up to 100us)
  bool bit = SDA_READ();
  twi_delay(twi_dcount);
  return bit;
}

static bool twi_write_byte(unsigned char byte) {
  
  if (byte == 0x43) {
    // printf("TWB %02x ", (uint32_t) byte);
    // do_log = true;
  }
  unsigned char bit;
  for (bit = 0; bit < 8; bit++) {
    twi_write_bit((byte & 0x80) != 0);
    byte <<= 1;
  }
  if (do_log) {
    printf("\n");
    do_log = false;
  }
  return !twi_read_bit();//NACK/ACK
}

static unsigned char twi_read_byte(bool nack) {
  unsigned char byte = 0;
  unsigned char bit;
  for (bit = 0; bit < 8; bit++) byte = (byte << 1) | twi_read_bit();
  twi_write_bit(nack);
  return byte;
}

unsigned char twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop){
  unsigned int i;
  if(!twi_write_start()) return 4;//line busy
  if(!twi_write_byte(((address << 1) | 0) & 0xFF)) {
    if (sendStop) twi_write_stop();
    return 2; //received NACK on transmit of address
  }
  for(i=0; i<len; i++) {
    if(!twi_write_byte(buf[i])) {
      if (sendStop) twi_write_stop();
      return 3;//received NACK on transmit of data
    }
  }
  if(sendStop) twi_write_stop();
  i = 0;
  while(SDA_READ() == 0 && (i++) < 10){
    SCL_LOW();
    twi_delay(twi_dcount);
    SCL_HIGH();
    twi_delay(twi_dcount);
  }
  return 0;
}

unsigned char twi_readFrom(unsigned char address, unsigned char* buf, unsigned int len, unsigned char sendStop){
  unsigned int i;
  if(!twi_write_start()) return 4;//line busy
  if(!twi_write_byte(((address << 1) | 1) & 0xFF)) {
    if (sendStop) twi_write_stop();
    return 2;//received NACK on transmit of address
  }
  for(i=0; i<(len-1); i++) buf[i] = twi_read_byte(false);
  buf[len-1] = twi_read_byte(true);
  if(sendStop) twi_write_stop();
  i = 0;
  while(SDA_READ() == 0 && (i++) < 10){
    SCL_LOW();
    twi_delay(twi_dcount);
    SCL_HIGH();
    twi_delay(twi_dcount);
  }
  return 0;
}

twi.h

/* 
  twi.h - Software I2C library for ESP31B

  Copyright (c) 2015 Hristo Gochkov. All rights reserved.
  This file is part of the ESP31B core for Arduino environment.
 
  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/
#ifndef SI2C_h
#define SI2C_h

#ifdef __cplusplus
extern "C" {
#endif

void twi_init(unsigned char sda, unsigned char scl);
void twi_stop(void);
void twi_setClock(unsigned int freq);
uint8_t twi_writeTo(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop);
uint8_t twi_readFrom(unsigned char address, unsigned char * buf, unsigned int len, unsigned char sendStop);

#ifdef __cplusplus
}
#endif

#endif