Add channnel shuffler

src/lmic/lmic_channelshuffle.c

@@ -0,0 +1,217 @@
+/*
+
+Module:	lmic_channelshuffle.c
+
+Function:
+    Channel scheduling without replacement.
+
+Copyright and License:
+    This file copyright (C) 2021 by
+
+        MCCI Corporation
+        3520 Krums Corners Road
+        Ithaca, NY  14850
+
+    See accompanying LICENSE file for copyright and license information.
+
+Author:
+    Terry Moore, MCCI Corporation	April 2021
+
+*/
+
+#include "lmic.h"
+#include <string.h>
+
+/****************************************************************************\
+|
+|   Manifest constants and local declarations.
+|
+\****************************************************************************/
+
+static unsigned sidewaysSum16(const uint16_t *pMask, uint16_t nEntries);
+static unsigned findNthSetBit(const uint16_t *pMask, uint16_t bitnum);
+
+/****************************************************************************\
+|
+|   Read-only data.
+|
+\****************************************************************************/
+
+/****************************************************************************\
+|
+|   Variables.
+|
+\****************************************************************************/
+
+/*
+
+Name:	LMIC_findNextChannel()
+
+Function:
+    Scan a shuffle mask, and select a channel (without replacement).
+
+Definition:
+    int LMIC_findNextChannel(
+                uint16_t *pShuffleMask,
+                const uint16_t *pEnableMask,
+                uint16_t nEntries,
+                int lastChannel
+                );
+
+Description:
+    pShuffleMask and pEnableMask are bit vectors. Channels correspond to
+    bits in little-endian order; entry [0] has channels 0 through 15, entry
+    [1] channels 16 through 31, and so forth. nEntries specifies the number
+    of entries in the mask vectors.  The enable mask is 1 for a given channel
+    if that channel is eligible for selection, 0 otherwise.
+
+    This routine selects channels from the shuffle mask until all entries
+    are exhausted; it then refreshes the shuffle mask from the enable mask.
+
+    If it refreshes the channel mask, lastChannel is taken as a channel number
+    that is to be avoided in the next selection. (This is to avoid back-to-back
+    use of a channel across a refresh boundary.) Otherwise lastChannel is
+    ignored. This avoidance can be suppresed by setting lastChannel to -1.
+    If only one channel is enabled, lastChannel is also ignored. If lastChannel
+    is actually disabled, lastChannel is also ignored.
+
+Returns:
+    A channel number, in 0 .. nEntries-1, or -1 if the enable mask is
+    identically zero.
+
+Notes:
+    This routine is somewhat optimized for AVR processors, which don't have
+    multi-bit shifts.
+
+*/
+
+int LMIC_findNextChannel(
+    uint16_t *pShuffleMask,
+    const uint16_t *pEnableMask,
+    uint16_t nEntries,
+    int lastChannel
+) {
+    unsigned nSet16;
+    uint16_t saveLastChannelVal;
+
+    // in case someone has changed the enable mask, update
+    // the shuffle mask so there are no disable bits set.
+    for (unsigned i = 0; i < nEntries; ++i) {
+        pShuffleMask[i] &= pEnableMask[i];
+    }
+
+    // count the set bits in the shuffle mask (with a factor of 16 for speed)
+    nSet16 = sidewaysSum16(pShuffleMask, nEntries);
+
+    // if zero, copy the enable mask to the shuffle mask, and recount
+    if (nSet16 == 0) {
+        memcpy(pShuffleMask, pEnableMask, nEntries * sizeof(*pShuffleMask));
+        nSet16 = sidewaysSum16(pShuffleMask, nEntries);
+    } else {
+        // don't try to skip the last channel becuase it can't be chosen.
+        lastChannel = -1;
+    }
+
+    // if still zero, return -1.
+    if (nSet16 == 0) {
+        return -1;
+    }
+
+    // if we have to skip a channel, and we have more than one choice, turn off
+    // the last channel bit. Post condition: if we really clered a bit,
+    // saveLastChannelVal will be non-zero.
+    saveLastChannelVal = 0;
+    if (nSet16 > 16 && lastChannel >= 0 && lastChannel <= nEntries * 16) {
+        uint16_t const saveLastChannelMask = (1 << (lastChannel & 0xF));
+
+        saveLastChannelVal = pShuffleMask[lastChannel >> 4] & saveLastChannelMask;
+        pShuffleMask[lastChannel >> 4] &= ~saveLastChannelMask;
+
+        // if we cleared a bit, reduce the count.
+        if (saveLastChannelVal > 0)
+            nSet16 -= 16;
+    }
+
+    if (saveLastChannelVal == 0) {
+        // We didn't eliminate a channel, so we don't have to worry.
+        lastChannel = -1;
+    }
+
+    // get a random number
+    unsigned choice = os_getRndU2() % ((uint16_t)nSet16 >> 4);
+
+    // choose a bit based on set bit
+    unsigned channel = findNthSetBit(pShuffleMask, choice);
+    pShuffleMask[channel / 16] ^= (1 << (channel & 0xF));
+
+    // handle channel skip
+    if (lastChannel >= 0) {
+        pShuffleMask[lastChannel >> 4] |= saveLastChannelVal;
+    }
+    return channel;
+}
+
+static unsigned sidewaysSum16(const uint16_t *pMask, uint16_t nEntries) {
+    unsigned result;
+
+    result = 0;
+    for (; nEntries > 0; --nEntries, ++pMask)
+        {
+        uint16_t v = *pMask;
+
+        // the following is an adaptation of Knuth 7.1.3 (62). To avoid
+        // lots of shifts (slow on AVR, and code intensive) and table lookups,
+        // we sum popc * 16, then divide by 16.
+
+        // sum adjacent bits, making a series of 2-bit sums
+        v = v - ((v >> 1) & 0x5555u);
+        v = (v & 0x3333u) + ((v >> 2) & 0x3333u);
+        // this assumes multiplies are essentialy free; 
+        v = (v & 0xF0F0u) + ((v & 0x0F0Fu) * 16u);
+        // Accumulate result, but note it's times 16.
+        // AVR compiler should optimize the x8 shift.
+        result += (v & 0xFF) + (v >> 8);
+        }
+
+    //
+    return result;
+}
+
+static unsigned findNthSetBit(const uint16_t *pMask, uint16_t bitnum) {
+    unsigned result;
+    result = 0;
+    bitnum = bitnum * 16;
+    for (;; result += 16) {
+        uint16_t m = *pMask++;
+        if (m == 0)
+            continue;
+        uint16_t v = m - ((m >> 1) & 0x5555u);
+        v = (v & 0x3333u) + ((v >> 2) & 0x3333u);
+        // this assumes multiplies are essentialy free; 
+        v = (v & 0xF0F0u) + ((v & 0x0F0Fu) * 16u);
+        // Accumulate result, but note it's times 16.
+        // AVR compiler should optimize the x8 shift.
+        v = (v & 0xFF) + (v >> 8);
+        if (v <= bitnum)
+            bitnum -= v;
+        else {
+            // the selected bit is in this word. We need to count.
+            while (bitnum > 0) {
+                m &= m - 1;
+                bitnum -= 16;
+            }
+            // now the lsb of m is our choice.
+            // get a mask, then use Knuth 7.1.3 (59) to find the
+            // bit number.
+            m &= -m;
+            result += ((m & 0x5555u) ? 0 : 1)
+                    + ((m & 0x3333u) ? 0 : 2)
+                    + ((m & 0x0F0Fu) ? 0 : 4)
+                    + ((m & 0x00FFu) ? 0 : 8)
+                    ;
+            break;
+        }
+    }
+
+    return result;
+}