PiSignals.java

package org.wpilib.hardware.hal.PiNotifier;
 
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.ReentrantLock;

public final class PiSignals {
  public static final int TYPE_EVENT = 0x01;

  public static final int TYPE_SEMAPHORE = 0x02;

  public static final int TYPE_NOTIFIER = 0x13;
 
  private static final int ERROR_BIT = 0x80000000;
 
  private static final class State {
    int signaled;
    boolean autoReset;
    int maxCount; // semaphores only
    final List<Condition> waiters = new ArrayList<>(2);
  }
 
  private static final ReentrantLock s_lock = new ReentrantLock();
  private static final Map<Integer, State> s_states = new HashMap<>();
  private static final Map<Integer, AtomicInteger> s_nextIndex = new HashMap<>();
 
  private PiSignals() {}
 
  // ---------------------------------------------------------------- handles

  public static int allocateHandle(int type) {
    AtomicInteger counter;
    s_lock.lock();
    try {
      counter = s_nextIndex.computeIfAbsent(type, t -> new AtomicInteger());
    } finally {
      s_lock.unlock();
    }
    int index = counter.incrementAndGet() & 0xFFFFFF;
    if (index == 0) {
      index = counter.incrementAndGet() & 0xFFFFFF; // skip 0 on wraparound
    }
    return ((type & 0x7F) << 24) | index;
  }
 
  // ---------------------------------------------------------- signal objects

  public static void createSignalObject(int handle, boolean manualReset, boolean initialState) {
    s_lock.lock();
    try {
      State s = s_states.computeIfAbsent(handle, h -> new State());
      s.signaled = initialState ? 1 : 0;
      s.autoReset = !manualReset;
    } finally {
      s_lock.unlock();
    }
  }

  public static void setSignalObject(int handle) {
    s_lock.lock();
    try {
      State s = s_states.get(handle);
      if (s == null) {
        return;
      }
      s.signaled = 1;
      // Wake everyone; with auto-reset only the first to re-check consumes it, the rest re-wait.
      for (Condition c : s.waiters) {
        c.signalAll();
      }
    } finally {
      s_lock.unlock();
    }
  }

  public static void resetSignalObject(int handle) {
    s_lock.lock();
    try {
      State s = s_states.get(handle);
      if (s != null) {
        s.signaled = 0;
      }
    } finally {
      s_lock.unlock();
    }
  }

  public static void destroySignalObject(int handle) {
    s_lock.lock();
    try {
      State s = s_states.remove(handle);
      if (s != null) {
        for (Condition c : s.waiters) {
          c.signalAll();
        }
      }
    } finally {
      s_lock.unlock();
    }
  }
 
  // ------------------------------------------------------------------ events

  public static int makeEvent(boolean manualReset, boolean initialState) {
    int handle = allocateHandle(TYPE_EVENT);
    createSignalObject(handle, manualReset, initialState);
    return handle;
  }

  public static void destroyEvent(int handle) {
    if ((handle >>> 24) == TYPE_EVENT) {
      destroySignalObject(handle);
    }
  }

  public static void setEvent(int handle) {
    if ((handle >>> 24) == TYPE_EVENT) {
      setSignalObject(handle);
    }
  }

  public static void resetEvent(int handle) {
    if ((handle >>> 24) == TYPE_EVENT) {
      resetSignalObject(handle);
    }
  }
 
  // -------------------------------------------------------------- semaphores

  public static int makeSemaphore(int initialCount, int maximumCount) {
    int handle = allocateHandle(TYPE_SEMAPHORE);
    s_lock.lock();
    try {
      State s = new State();
      s.signaled = initialCount;
      s.autoReset = true;
      s.maxCount = maximumCount;
      s_states.put(handle, s);
    } finally {
      s_lock.unlock();
    }
    return handle;
  }

  public static void destroySemaphore(int handle) {
    if ((handle >>> 24) == TYPE_SEMAPHORE) {
      destroySignalObject(handle);
    }
  }

  public static boolean releaseSemaphore(int handle, int releaseCount) {
    if ((handle >>> 24) != TYPE_SEMAPHORE || releaseCount <= 0) {
      return false;
    }
    s_lock.lock();
    try {
      State s = s_states.get(handle);
      if (s == null || (s.maxCount - s.signaled) < releaseCount) {
        return false;
      }
      s.signaled += releaseCount;
      for (Condition c : s.waiters) {
        c.signalAll();
      }
      return true;
    } finally {
      s_lock.unlock();
    }
  }
 
  // ------------------------------------------------------------------ waits
  // These four match the WPIUtilJNI Java signatures and exception behavior.

  public static void waitForObject(int handle) throws InterruptedException {
    int[] r = waitCore(new int[] {handle}, -1, null);
    if (r.length == 0 || (r[0] & ERROR_BIT) != 0) {
      throw new InterruptedException("WaitForObject interrupted");
    }
  }

  public static boolean waitForObjectTimeout(int handle, double timeout)
      throws InterruptedException {
    boolean[] timedOut = new boolean[1];
    int[] r = waitCore(new int[] {handle}, timeout, timedOut);
    if (timedOut[0]) {
      return true;
    }
    if (r.length == 0 || (r[0] & ERROR_BIT) != 0) {
      throw new InterruptedException("WaitForObject interrupted");
    }
    return false;
  }

  public static int[] waitForObjects(int[] handles) throws InterruptedException {
    int[] r = waitCore(handles, -1, null);
    if (r.length == 0) {
      throw new InterruptedException("WaitForObjects interrupted");
    }
    return r;
  }

  public static int[] waitForObjectsTimeout(int[] handles, double timeout)
      throws InterruptedException {
    boolean[] timedOut = new boolean[1];
    int[] r = waitCore(handles, timeout, timedOut);
    if (r.length == 0 && !timedOut[0]) {
      throw new InterruptedException("WaitForObjects interrupted");
    }
    return r;
  }
 
  private static int[] waitCore(int[] handles, double timeout, boolean[] timedOutOut)
      throws InterruptedException {
    s_lock.lock();
    Condition cond = null;
    try {
      long deadline = timeout >= 0 ? System.nanoTime() + (long) (timeout * 1e9) : 0;
      boolean timedOut = false;
      int[] out = new int[handles.length];
      int count;
      for (;;) {
        count = 0;
        for (int h : handles) {
          State s = s_states.get(h);
          if (s == null) {
            out[count++] = h | ERROR_BIT; // non-existent/destroyed handle counts as signaled+error
          } else if (s.signaled > 0) {
            out[count++] = h;
            if (s.autoReset) {
              s.signaled--;
            }
          }
        }
        if (count != 0 || timedOut) {
          break;
        }
        if (timeout == 0) {
          timedOut = true;
          break;
        }
        if (cond == null) {
          cond = s_lock.newCondition();
          for (int h : handles) {
            State s = s_states.get(h);
            if (s != null) {
              s.waiters.add(cond);
            }
          }
        }
        if (timeout < 0) {
          cond.await();
        } else {
          long remaining = deadline - System.nanoTime();
          if (remaining <= 0 || cond.awaitNanos(remaining) <= 0) {
            timedOut = true; // loop once more: a signal may have raced the timeout
          }
        }
      }
      if (timedOutOut != null) {
        timedOutOut[0] = timedOut && count == 0;
      }
      int[] result = new int[count];
      System.arraycopy(out, 0, result, 0, count);
      return result;
    } finally {
      if (cond != null) {
        for (int h : handles) {
          State s = s_states.get(h);
          if (s != null) {
            s.waiters.remove(cond);
          }
        }
      }
      s_lock.unlock();
    }
  }
}