astra/iphone-encoder-counter

firmware/counter.c

#include "counter.h"
#include <string.h>
static void sat64(uint64_t *n) { if (*n != UINT64_MAX) ++*n; }
static void sat32(uint32_t *n) { if (*n != UINT32_MAX) ++*n; }
void counter_init(counter_t *c,uint8_t phase) { memset(c,0,sizeof *c); c->phase=phase&3; }
void counter_rebase(counter_t *c,uint8_t phase) { c->phase=phase&3; c->pending=0; }
uint64_t counter_total_twists(const counter_t *c) { return UINT64_MAX-c->cw<c->ccw ? UINT64_MAX : c->cw+c->ccw; }
int counter_step(counter_t *c,uint8_t phase) {
    static const int8_t delta[16]={0,1,-1,2, -1,0,2,1, 1,2,0,-1, 2,-1,1,0};
    if (phase>3 || c->phase>3) return 0;
    int d=delta[(c->phase<<2)|phase];
    c->phase=phase;
    if(d==2) { sat32(&c->invalid_transitions);c->pending=0;return 0; }
    c->pending=(int8_t)(c->pending+d);
    if(c->pending>=4) { sat64(&c->ccw);c->pending-=4;return 1; }
    if(c->pending<=-4) { sat64(&c->cw);c->pending+=4;return -1; }
    return 0;
}
static uint32_t crc32(const uint8_t *p,size_t n) {
    uint32_t crc=UINT32_MAX;
    for(size_t i=0;i<n;i++) { crc^=p[i];for(int b=0;b<8;b++) crc=(crc>>1)^(0xEDB88320u & (0u-(crc&1u))); }
    return ~crc;
}
static void put(uint8_t *p,uint64_t v,unsigned n) { for(unsigned i=0;i<n;i++){p[i]=(uint8_t)v;v>>=8;} }
static uint64_t get(const uint8_t *p,unsigned n) { uint64_t v=0;for(unsigned i=0;i<n;i++)v|=(uint64_t)p[i]<<(8*i);return v; }
void journal_encode(uint8_t out[JOURNAL_BYTES],const counter_t *c,uint32_t seq) {
    memset(out,0,JOURNAL_BYTES);memcpy(out,"FDG2",4);
    put(out+4,seq,4);put(out+8,c->cw,8);put(out+16,c->ccw,8);
    put(out+24,c->presses,8);put(out+32,c->invalid_transitions,4);out[36]=(uint8_t)(c->pending+3);out[37]=c->phase;
    put(out+40,crc32(out,40),4);out[46]=0x5A;out[47]=0xA5;
}
bool journal_decode(const uint8_t in[JOURNAL_BYTES],counter_t *c,uint32_t *seq) {
    if(memcmp(in,"FDG2",4)||in[46]!=0x5A||in[47]!=0xA5||
       get(in+40,4)!=crc32(in,40)||in[36]>6||in[37]>3||
       in[38]||in[39]||in[44]||in[45])return false;
    *seq=(uint32_t)get(in+4,4);c->cw=get(in+8,8);c->ccw=get(in+16,8);
    c->presses=get(in+24,8);c->invalid_transitions=(uint32_t)get(in+32,4);c->pending=(int8_t)in[36]-3;c->phase=in[37];return true;
}
int journal_restore(const uint8_t a[JOURNAL_BYTES],const uint8_t b[JOURNAL_BYTES],counter_t *c,uint32_t *seq) {
    counter_t ca,cb;uint32_t sa=0,sb=0;bool va=journal_decode(a,&ca,&sa),vb=journal_decode(b,&cb,&sb);
    if(!va&&!vb)return -1;
    uint32_t age=sb-sa;
    if(vb&&(!va||(age!=0&&age<0x80000000u))){*c=cb;*seq=sb;return 1;}
    *c=ca;*seq=sa;return 0;
}

void button_init(button_t *b,bool down,uint32_t now_ms) {
    b->stable=b->candidate=down;b->armed=!down;b->since=now_ms;
}
bool button_update(button_t *b,counter_t *c,bool down,uint32_t now_ms) {
    if(down!=b->candidate){b->candidate=down;b->since=now_ms;}
    if(b->candidate==b->stable || (uint32_t)(now_ms-b->since)<20)return false;
    b->stable=b->candidate;
    if(!b->stable){b->armed=true;return false;}
    if(b->armed){b->armed=false;sat64(&c->presses);return true;}
    return false;
}