Atomic Vector (draft)

/**
Copyright 2020 Frederick Woodruff

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <atomic>
#include <type_traits>
#include <cstdlib.h>

#pragma once

namespace fbw {

template<int unused>
std::atomic<void*>& hazard_pointer_for_this_thread();

template<int id> void release_hazard();
template<typename T> void reclaim(T*);
void delete_nodes_with_no_hazards();

template <template<class> typename H, typename T>
class fetch_op_support;

template<typename T>
class atomic_vector {
private:
  buffer_handle load_buffers();
  void release_buffers();
  size_t capacity_hint();
  bool cas_buffers(size_t& expected_capacity, buffer*);
  bool cas_index_weak(size_t, T&, T);
  bool cas_index_strong(size_t, T&, T);
  struct buffer;
  struct buffer_handle;
  struct pack;
  static constexpr size_t INITIAL_CAPACITY = 30;
public:
  class  atomic_reference;

  atomic_vector(const atomic_vector&) = delete;
  atomic_vector operator=(const atomic_vector&) = delete;

  static constexpr bool is_always_lock_free =
    std::atomic<pack>::is_always_lock_free &&
    std::atomic<buffer_handle>::is_always_lock_free;

  atomic_vector() {
    auto* buff = new (INITIAL_CAPACITY) buffer;
    atomic_handle.store({buff,nullptr});
    size.store(0);
  }

  ~atomic_vector() {
    auto buffers = atomic_handle.load();
    assert(!buffers.old_ptr);
    delete buffers.ptr;
  }

  /* returns index object was placed at.
     This lets us use the container as a
     wait-free stack if a sufficiently
     large buffer has been reserved.

     push_back not possible because
     atomics are not moveable.
  */
  size_t emplace_back(T val) {
    auto expected_cap = capacity_hint();
    auto idx = size.fetch_add(1);
    auto new_cap = max(idx+1,expected_cap*2);
    if(idx > expected_cap) {
       auto* pbuffer = new (new_cap) buffer;
       bool success_code;
       do {
         success_code = cas_buffers(expected_cap, pbuffer);
       } while (idx > expected_cap && !success_code
               && size.load() > idx);
       if(!success_code) {
          delete buff;
       }
    }
    T temp;
    cas_index_strong(idx, temp, temp);
    cas_index_strong(idx, temp, val);
  }

  void pop_back() {
    size--;
  }

  void reserve(size_t capacity) {
    auto expected_cap = capacity_hint();
    if(capacity > expected_cap) {
       auto* pbuffer = new(capacity) buffer;
       bool success_code;
       do {
         success_code = cas_buffers(expected_cap, pbuffer);
       } while (capacity > expected_cap && !success_code);
       if(!success_code) {
          delete buff;
       }
  }

  void resize(size_t siz) {
    reserve(siz);
    size.store(siz);
  }

  void shrink_to_fit() {
    auto idx = size.load();
    if(idx < cap) {
       auto* shrunk_buffer = new(idx) buffer;
       if(!cas_buffers(cap, shrunk_buffer)) {
          delete shrunk_buffer;
       }
    }
  }

  size_t size() {
    auto s = size.load();
    return max(s,0);
  }

  /*
   we lose linearisability
   but not correctness when shrink_to_fit
   and emplace_back ard called concurrently
  */
  size_t capacity() {
    return capacity_hint();
  }

  void clear() {
    size.store(0);
    auto* buff = new(1) buffer;
    while(!cas_buffers)
  }

  atomic_reference operator[](size_t idx) {
    return atomic_reference(idx,this);
  }

  atomic_reference back() {
    auto idx = size.load()-1;
    assert(idx > 0);
    return atomic_reference(idx,this);

  atomic_reference front() {
    return atomic_reference(0,this);
  }

  void assign(size_t count, const T& value) {
     size_t cap = (count*3)/2 + 1;
     auto* pbuffer = new(cap) buffer;
     for(int i = 0; i < count; ++i) {
        pbuffer->array[i] = {value, state::active};
     }
     auto expected_cap = capacity_hint();
     while(!cas_buffers(expected_cap, pbuffer));
     size.store(count);
  }


  class atomic_reference : fetch_op_support<atomic_reference> {
    size_t idx;
    atomic_vector* v;
  public:
    atomic_reference(const atomic_reference&) = delete;
    atomic_reference
        operator=(const atomic_reference&) = delete;

    bool compare_exchange_weak(
             T& expected,
             T desired) {
      assert(idx < v->size.load());
      return v->cas_index_weak(idx, expected, desired)==0;
    }

    bool compare_exchange_strong(
             T& expected,
             T desired) {
       assert(idx < v->size.load());
       return v->cas_index_strong(idx, expected, desired)==0;
    }

    T load() {
      T ret;
      compare_exchange_strong(ret,ret);
      return ret;
    }

    operator T() {
      return load();
    }

    T exchange(T new_value) {
      T temp;
      while(!compare_exchange_weak(temp, new_value));
      return temp;
    }

    void store(T new_value) {
      return (void)exchange(new_value);
    }
  };

  friend class atomic_reference;

private:
  std::atomic<buffer_handle> atomic_handle;
  std::atomic<intptr_t> size;
  struct awaitable_ptr {
    awaitable* ptr;
    long count;
  };

  void push_awaitable(awaitable* waitable) {
     awaitable_ptr old_head = awaitables.load();
     do {
        waitable->next = old_head.ptr;
     while(awaitables.compare_exchange_weak(old_head,
                                            {waitable,1});
  }

  bool pop_awaitable(T value) {
    awaitable_ptr old_head = awaitables.load();
    if(!old_head.ptr) {
       return false;
    }
    while(true) {
       while(!awaitables.
        compare_exchange_weak(old_head,{old_head.ptr,
                                        old_head.count+1);
       if(!old_head.ptr) {
           return false;
       }
       if(awaitables.
         compare_exchange_strong(old_head,
                                old_head.ptr->next) {
          /* here we have ownership of the awaitable
             and can set its value but it is not safe
             to resume the coroutine because another
             thread may pathologically dereference
             old_head.ptr->next after its coroutine
             frame has been deleted
          */
          old_head.ptr->value = value;
          long increase = old_head.count-2;
          if(!old_head.ptr->count+=increase) {
            old_head.ptr->co_handle.resume();
          }
          return true;
       } else if(!ptr->internal_count--) {
          old_head.ptr->co_handle.resume();
       }
    }
  }

  struct buffer_handle {
    buffer* ptr;
    buffer* old_ptr;
  };

  enum class state : unsigned char {
      active,
      locked,
      uninitialised
  };

  struct pack {
    T value;
    state mark;
  };

  struct buffer {
    size_t capacity;
    atomic<pack> array[1];
    static void* operator new(size_t sz, size_t capacity) {
       buffer* pbuff =
        ::operator new(sz +
               (min(capacity-1,1)) * sizeof(atomic<pack>);
      pbuff->array =
          new (&pbuff->array[0]) atomic<pack>[capacity];
      for(int i = 0; i < capacity; ++i) {
         pbuff->array[i].store({T(), state::uninitialised});
      }
      pbuff->capacity = capacity;
      return pbuff;
    }
  };


  buffer_handle sync_buffer() {
      auto buffers = load_buffers();
      while(buffers.old_ptr) {
        transfer_buffers(buffers.old_ptr,
                       buffers.ptr);
        release_buffers();
        if(atomic_buffer.compare_exchange_strong(
              buffer,
             {buffer.ptr, nullptr})) {

           reclaim(buffer.old_ptr);
           buffer.old_ptr = nullptr;
           return buffers;
        }
        buffers = load_buffers();
     }
   }


  bool cas_buffer(size_t& expected
                           buffer* new_buffer) {
    auto cap  = capacity_hint();
    if(cap != expected) {
      expected = cap;
      return false;
    }
    auto buffers = sync_buffer();
    buffer_handle new_buffers = {new_buffer, buffers.ptr};

    if(!atomic_buffer.compare_exchange_strong(
                        buffers,
                        new_buffers) {
        expected = capacity_hint();
        return false;
    }
    sync_buffer();
    delete_nodes_with_no_hazards();
    return true;
  }

  template<bool strong>
  bool cas_index(size_t idx,
                 T& expected,
                 T desired) {
    while(true) {
      auto buffers = load_buffers();
      if(buffers.ptr->capacity < idx) {
        release_buffers();
        expected = T();
        return false;
      }
      if(buffers.ptr) {
        transfer_buffer_index(idx, buffers.old_ptr,
                              buffers.ptr);
      }
      release_old_buffer();
      pack mark_expected = { expected, state::active };
      pack mark_desired  = { desired,  state::active };
      auto& ref = buffers.ptr->array[idx];
      if constexpr (strong) {
        if(ref.compare_exchange_strong(mark_expected,
                                       mark_desired)) {
          release_new_buffer();
          return true;
        }
      } else {
        if(ref.compare_exchange_weak(mark_expected,
                                     mark_desired)) {
          release_new_buffer();
          return true;
        }
      }
      release_new_buffer();
      expected = mark_expected.value;
      if(mark_expected.mark == state::active) {
        return false;
      }
    }
  }

  bool cas_index_weak(size_t idx, T& expected, T desired) {
    return cas_index<false>(idx, expected, desired);
  }

  bool cas_index_strong(size_t idx, T& expected, T desired) {
    return cas_index<true>(idx, expected, desired);
  }


  void transfer_buffer_index(size_t idx, buffer* old_buffer,
                           buffer* new_buffer) {
    auto cap = old_buffer->capacity;
    pack old_val = (idx<cap)?
               old_buffer->array[idx].load() :
               {nullptr, state::locked};
    if (idx<cap) {
       while(old_val.mark == state::active &&
            !old_buffer->array[idx].compare_exchange_weak(
                old_val, {old_val.value, state::locked});
     }
     pack new_val = new_buffer->array[idx].load();

     while(new_val.mark == state::uninitialised &&
           !new_buffer->array[idx].compare_exchange_weak(
               new_val, {old_val.value, state::active}));
 }

 /* TODO after profiling:
    Each thread should perform transfers on different
    large chunks of values then CAS the first marker from each
    chunk from state::locked to state::block_complete.
    Reading off each 'block_complete' marker,
    any thread can quickly skip through, determining
    what needs to be done to finish transferring a block.

    Alternatively I may find that allocations
    are my bottleneck or maybe memory ordering.

    Replacing the marked values with immutable pointers would
    allow larger lock-free types and fetch_add support but
    involves lots of dereferencing.

    I should find a motivating use-case then implement
    iterator-based methods like insert() using some
    similar 'marking' scheme.
 */

 void transfer_buffers(buffer* old_buffer,
                       buffer* new_buffer) {
   for(int i = 0; i < new_buffer->capacity; ++i) {
       transfer_buffer_index(idx, old_buffer, new_buffer};
   }
 }

  void transfer_block(int block_size, int block_id,
                     buffer* old_buffer, buffer* new_buffer) {
     size_t start = block_size*block_id;
     size_t end = min(new_buffer->capacity, start+block_size)
     auto& ref = old_buffer->array[start];
     auto val = ref.load();
     if(val.mark == state::block_complete) {
        return;
     }

     for(size_t i = start; i < end, ++i) {
        transfer_buffer_index(i,old_buffer, new_buffer);
     }
     val = ref.load();
     ref.store({val.value, state::block_complete});
  }

 size_t capacity_hint() {
   auto cap = load_new_buffer()->capacity;
   release_new_buffer();
   return cap;
 }

buffer* load_new_buffer() {
    auto buffers = atomic_handle.load();
    do {
      auto temp = buffers;
      auto& hp = hazard_ptr_for_this_thread<0>();
      hp0.store(temp.ptr);
      buffers = atomic_handle.load();
    } while (buffers!=temp);
    return buffers.ptr;
}

 buffer_handle load_buffers() {
  auto buffers = atomic_handle.load();
  do {
   auto temp = buffers;
   auto& hp0 = hazard_ptr_for_this_thread<0>();
   auto& hp1 = hazard_ptr_for_this_thread<1>();
   hp0.store(temp.ptr);
   hp1.store(temp.old_ptr);
   buffers = atomic_handle.load();
  } while (buffers!=temp);
  return buffers;
 }

 void release_buffers() {
   release_hazard<0>();
   release_hazard<1>();
 }

 void release_new_buffer() {
   release_hazard<0>();
 }

 void release_old_buffer() {
   release_hazard<1>();
 }
};

// Most of this is from Anthony William's
// C++ Concurrency in Action
// I have replaced the fixed size array of hazard
// pointers with a linked block list to avoid an artificial
// limit on the number of threads

struct hazard_pointer {
    std::atomic<std::thread::id> id;
    std::atomic<void*> pointer;
};

// this should be a soft upper bound on the
// number of active threads at any one time x2
constexpr size_t HAZARD_BLOCK = 64;

struct hazard_block {
    hazard_pointer block[HAZARD_BLOCK];
    atomic<hazard_pointers*> next = nullptr;
}

hazard_block hazard_ptrs;

class hp_owner {
    hazard_pointer* hp;
public:
    hp_owner(hp_owner const&)=delete;
    hp_owner operator=(hp_owner const&)=delete;

    hp_owner() : hp(nullptr) {
      hazard_block* hb = &hazard_ptrs;
      do {
        for(int i = 0; i < HAZARD_BLOCK; ++i) {
          std::thread::id old_id;
          if(hb->block[i].id.compare_exchange_strong(
              old_id, std::this_thread::get_id())) {
            hp = hazard_pointers[i];
            return;
          }
        }
        auto* temp = hb->next.load();
        if(temp) {
            hb = temp;
        } else {
          hazard_block* new_block = new hazard_block();
          if(hb->next.compare_exchange_strong(
              temp, new_block)) {
            hb = new_block;
          } else {
            delete new_block;
            hb = temp;
          }
        }
      } while(true);
    }

    std::atomic<void*>& get_pointer() {
       return hp->pointer;
    }

    ~hp_owner() {
      hp->pointer.store(nullptr);
      hp->id.store(std::thread::id());
    }
};

template<int unused>
std::atomic<void*>& hazard_pointer_for_this_thread() {
  thread_local hp_owner hazard;
  return hazard.get_pointer();
}

template<int id>
void release_hazard() {
  auto& hp = hazard_pointer_for_this_thread<id>();
  hp.store(nullptr);
}

template<typename T>
void do_delete(void* p)
{
    delete static_cast<T*>(p);
}

struct data_to_reclaim {
    void* data;
    std::function<void(void*)> deleter;
    data_to_reclaim* next

    template<typename T>
    data_to_reclaim(T* p) : data(p),
                            deleter(&do_delete<T>),
                            next(0) {}

    ~data_to_reclaim() {
      deleter(data);
    }
};

bool outstanding_hazard_pointers_for(void* p) {
    for(unsigned i=0;i<max_hazard_pointers;++i) {
      if(hazard_pointers[i].pointer.load()==p) {
         return true;
      }
    }
    return false;
}

std::atomic<data_to_reclaim*> nodes_to_reclaim;

void add_to_reclaim_list(data_to_reclaim* node) {
  node->next=nodes_to_reclaim.load();
  while(!nodes_to_reclaim.
        compare_exchange_weak(node->next,node));
}

template<typename T>
void reclaim(T* data) {
    if(outstanding_hazard_pointers_for(data)) {
      add_to_reclaim_list(new data_to_reclaim(data));
    } else {
      delete data;
    }
}

void delete_nodes_with_no_hazards() {
  data_to_reclaim* current=nodes_to_reclaim.exchange(nullptr);
  while(current) {
    data_to_reclaim* const next=current->next;
    if(!outstanding_hazard_pointers_for(current->data)) {
      delete current;
    } else {
      add_to_reclaim_list(current);
    }
    current=next;
  }
}

// support for fetch_add etc.

template <template<class> typename U, typename T>
struct fetch_op_support {
    using Arg = std::conditional_t<
                    std::is_integral_v<T>, T, intptr_t>
    using Arg2 = std::enable_if_t<
                    std::is_integral_v<T>, T>
    using Ret = std::conditional_t<
                   std::is_integral_v<T>, T,
                std::enable_if_t<
                 std::is_pointer_v<T>, T> >
private:
    template<typename A, typename Op>
    Ret op(A value, Op&& mod) {
        Ret expected;
        while(!
           static_cast<U<T>*>(this)->
            compare_exchange_weak(
            expected, mod(expected, value));
        return expected;
    }

public:
Ret fetch_add(Arg v) {return op(v,[](Ret a, Arg b){a + b;});}
Ret fetch_sub(Arg v) {return op(v,[](Ret a, Arg b){a - b;});}
Ret fetch_and(Arg2 v){return op(v,[](Ret a,Arg2 b){a & b;});}
Ret fetch_or (Arg2 v){return op(v,[](Ret a,Arg2 b){a | b;});}
Ret fetch_xor(Arg2 v){return op(v,[](Ret a,Arg2 b){a ^ b;});}
    Ret operator+=(Arg  val) { return fetch_add(val) + val; }
    Ret operator-=(Arg  val) { return fetch_sub(val) - val; }
    Ret operator&=(Arg2 val) { return fetch_and(val) & val; }
    Ret operator|=(Arg2 val) { return fetch_or (val) | val; }
    Ret operator^=(Arg2 val) { return fetch_xor(val) ^ val; }
    Ret operator++()    { return *this+=1; }
    Ret operator--()    { return *this-=1; }
    Ret operator++(int) { return *this++; }
    Ret operator--(int) { return *this--; }
}

}; // namespace fbw