TMF8828-Test.spin

{
----------------------------------------------------------------------------------------------------
    Filename:       TMF8828-Test.spin
    Description:    TMF8828 bootloader and application test
    Author:         Jesse Burt
    Started:        Dec 9, 2022
    Updated:        May 3, 2024
    Copyright (c) 2024 - See end of file for terms of use.
----------------------------------------------------------------------------------------------------
}
CON

    _clkmode = xtal1+pll16x
    _xinfreq = 5_000_000

    SLAVE_WR        = core.SLAVE_ADDR
    SLAVE_RD        = core.SLAVE_ADDR | 1
    EN_M            = 24
    INT_PIN         = 25


OBJ

    ser: "com.serial.terminal.ansi"
    time: "time"
    i2c: "com.i2c"
    core: "core.con.tmf8828"


VAR

    byte _ramdump[132]
    byte _fact_cal[192*4]


pub main() | tries, ena, appid, minor, patch, mode

    outa[EN_M] := 0
    dira[EN_M] := 1

    ser.init_def(115_200)
    time.msleep(30)
    ser.clear()
    ser.strln(@"serial started")

    i2c.init(28, 29, 4_000_000)
    time.msleep(30)
    ser.strln(@"i2c started")

' 1.2.1) Power up
    outa[EN_M] := 0                             ' cold start
    time.msleep(1)                              '
    outa[EN_M] := 1
    writereg(core.ENABLE, 1, $01)

' 2.1) Check that the device is ready for communication
    tries := 0
    repeat
        ena := 0
        readreg(core.ENABLE, 1, @ena)
        ++tries
    until (ena == $41)                          ' 2.1.2
    ser.printf1(@"success after %d tries\n\r", tries)

    ser.str(@"reading APPID: ")
    appid := 0
    readreg(core.APPID, 3, @appid)              ' read APPID, MINOR, PATCH
    minor := appid.byte[1]
    patch := appid.byte[2]
    appid &= $ff
    if ( appid == core.BL_RUNNING )
        ser.strln(@"bootloader running")
        if ( minor == $26 )
            ser.printf1(@"ROM v1, patch level %02.2x\n\r", patch)
        elseif ( minor == $29 )
            ' TMF882X ROM v2 requires firmware download before a valid measurement can be done (s 3.2)
            ' TMF8828 ROM v2 requires the download of an application patch.
            ser.printf1(@"ROM v2, patch level %02.2x\n\r", patch)
            fw_load()
        else
            ser.printf1(@"error: exception (%x)\n\r", minor)
            repeat
    elseif (appid == core.MEAS_APP_RUNNING)
        ser.strln(@"application running")
    else
        ser.printf1(@"error: exception (%x)\n\r", appid)
        repeat

    ' 2.4.1) Check the application mode
    mode := 0
    readreg(core.MODE, 1, @mode)
    if ( mode == $00 )
        ser.strln(@"Application is in TMF8821 mode")
    elseif ( mode == $08 )
        ser.strln(@"Application is in TMF8828 mode")
    else
        ser.printf1(@"error: unknown application mode %02.2x\n\r", mode)
        repeat

    configure()
    factory_cal_tmf8828()
    measure()


PUB fw_load() | img_remain, img_ptr, chunk_sz, img_csum, tmp, status, tries, aid
'3.2)
    ser.str(@"sending DOWNLOAD_INIT...")
    bl_command(core.DOWNLOAD_INIT, 1, $29)      ' $29: seed (not otherwise documented)
    ser.strln(@"done")

    bl_wait_rdy()

    ser.str(@"sending ADDR_RAM...")
    bl_command(core.ADDR_RAM, 2, $00_00)
    ser.strln(@"done")

    bl_wait_rdy()

    { load RAM }
    img_remain := _img_sz
    img_ptr := @tmf8828_image                   ' init pointer to start of FW image
    repeat
        chunk_sz := img_remain <# 128           ' chunk size is the remaining size, up to 128 bytes
        ser.pos_xy(0, 10)
        ser.printf1(@"about to write offset %x\n\r", img_ptr)
        i2c.start()
        i2c.write(SLAVE_WR)
        i2c.write(core.BL_CMD_STAT)
        i2c.write(core.W_RAM)
        i2c.write(chunk_sz)                     ' BL_SIZE (number of bytes to write; up to 128)
        i2c.wrblock_lsbf(img_ptr, chunk_sz)

        { calc checksum }
        img_csum := 0
        img_csum += core.W_RAM
        img_csum += chunk_sz
        repeat tmp from img_ptr to (img_ptr+(chunk_sz-1))
            img_csum += byte[tmp]               ' calc rolling checksum
        i2c.write(img_csum ^ $ff)               ' one's comp. so sum of 0 doesn't yield chksum of 0
        i2c.stop()

        ser.str(@"checking status of write...")
        status := 0
        readreg(core.BL_CMD_STAT, 3, @status)
        if (status <> $ff_00_00)    'XXX was 00 00 ff
            ser.printf1(@"WRITE FAILED: status %06.8x\n\r", status)
            img_ptr -= chunk_sz
            img_remain += chunk_sz
            ++tries
            if (tries > 10)                     ' 10: arbitrary
                ser.strln(@"number of retries exceeded maximum - halting")
                repeat
        else
            ser.fgcolor(ser.GREEN)
            ser.strln(@"write OK")
            ser.fgcolor(ser.GREY)
            img_ptr += chunk_sz
            img_remain -= chunk_sz
            tries := 0
        ser.printf1(@"remaining: %4.4d\n\r", img_remain)
    while img_remain
    ser.strln(@"COMPLETE!")

    ser.str(@"Sending RAMREMAP_RESET command...")
    bl_command(core.RAMREMAP_RESET, 0, 0)
    ser.strln(@"done")

    ser.str(@"checking APPID...")
    tries := 0
    repeat
        aid := 0
        readreg(core.APPID, 1, @aid)
        ser.printf1(@"$%02.2x ", aid)
        { should respond with $03 after no more than 2.5ms }
        time.usleep(500)
        if (++tries > 5)
            ser.strln(@"APPID read failed")
            repeat
    until (aid == core.MEAS_APP_RUNNING)
    ser.printf1(@"(%d tries)\n\r", tries)


PUB configure() | status, tmp, mask_nr
' 4.1
    ' step 1
    ser.str(@"Loading common config page...")
    command(core.CMD_LD_CFG_PG_COM)
    ser.strln(@"done")

    ' step 3
    ser.str(@"Verifying the config page is loaded...")
    if ( config_page_is_loaded(core.CMD_LD_CFG_PG_COM) )
        ser.fgcolor(ser.GREEN)
        ser.strln(@"verified")
        ser.fgcolor(ser.GREY)
    else
        ser.fgcolor(ser.RED)
        ser.strln(@"verification failed - halting")
        ser.fgcolor(ser.GREY)
        repeat

    ' step 4
    ser.str(@"changing measurement period to 100ms...")
    writereg(core.PERIOD_MS_LSB, 2, $00_64)
    ser.strln(@"done")

    ' step 5
    ser.str(@"selecting pre-defined SPAD mask #6...")
    writereg(core.SPAD_MAP_ID, 1, $06)
    ser.strln(@"done")

    ' step 6
    ser.str(@"config GPIO0 low while VCSEL is emitting...")
    writereg(core.GPIO_0, 1, $03)
    ser.strln(@"done")

    ' step 7
    ser.str(@"writing common page...")
    command(core.CMD_WRITE_CFG_PG)
    ser.strln(@"done")

    ' step 9
    ser.str(@"Enabling interrupts...")
    writereg(core.INT_ENAB, 1, $02) '$62 to include error/warning/cmd done interrupts
    ser.strln(@"done")

    ' step 10
    ser.str(@"Clear old pending interrupts...")
    writereg(core.INT_STATUS, 1, $ff)
    ser.strln(@"done")


PUB factory_cal_tmf8820_21() | tmp, status

'4.4.1) factory cal TMF8820/21
    ser.str(@"starting factory cal...")
    command(core.CMD_FACTORY_CAL)
    ser.strln(@"done")

    ser.str(@"loading factory calibration config page...")
    command(core.CMD_LD_CFG_PG_FACT_CAL)
    ser.strln(@"done")
    ser.str(@"reading factory cal...")
    readreg(core.CONFIG_RESULT, 192, @_fact_cal)
    ser.strln(@"done")

'4.4.2) load factory cal
    ser.str(@"loading factory calibration config page...")
    command(core.CMD_LD_CFG_PG_FACT_CAL)
    ser.strln(@"done")

    if ( config_page_is_loaded(core.CMD_LD_CFG_PG_FACT_CAL) )
        ser.strln(@"config page loaded")

    ser.str(@"writing calibration...")
    writereg(core.FACTORY_CALIBR_FIRST, 192-4, @_fact_cal+4)
    ser.strln(@"done")
    status := 0
    readreg(core.CALIBRATION_STATUS, 1, @status)
    if (status == core.WARN_NO_FACT_CALIBR)
        ser.strln(@"warning: no factory cal loaded")
    elseif (status == core.WARN_FACT_CAL_SPAD_MASK_MISMATCH)
        ser.strln(@"warning: factory cal doesn't match the selected SPAD map")

PUB factory_cal_tmf8828() | status, tmp, mask_nr
'4.4.3) TMF8828 factory cal
    ser.strln(@"TMF8828 factory calibration")
    ser.str(@"Loading common config page...")
    command(core.CMD_LD_CFG_PG_COM)
    ser.strln(@"done")

    ser.str(@"    Verifying the config page is loaded...")

    if ( config_page_is_loaded(core.COMMON_CID) )
        ser.strln(@"verified")
    else
        ser.strln(@"verification failed - halting")
        repeat

    ser.str(@"    writing common page...")
    command(core.CMD_WRITE_CFG_PG)
    ser.strln(@"done")

    command(core.CMD_RESET_FACTORY_CAL)         ' ensure we're starting at the first cal page
    repeat tmp from 1 to 4                      ' write the factory cal for each SPAD mask
        ser.printf1(@"    calibrating SPAD mask %d...", tmp)
        command(core.CMD_FACTORY_CAL)           ' writing the cal automatically advances the page
        ser.strln(@"done")


'steps 7..11: 4 times
    command(core.CMD_RESET_FACTORY_CAL)         ' ensure we're starting at the first cal page
    repeat mask_nr from 0 to 3
        ser.str(@"    loading factory calibration config page...")
        command(core.CMD_LD_CFG_PG_FACT_CAL)

        ser.printf1(@"reading factory cal for mask #%d...", mask_nr)
        readreg(core.CONFIG_RESULT, 192, @_fact_cal+(mask_nr*192))
        ser.strln(@"done")
        command(core.CMD_WRITE_CFG_PG)
    'ser.hexdump(@_fact_cal, $20, 2, 192, 16)
    'ser.newline
    'ser.hexdump(@_fact_cal+192, $20, 2, 192, 16)
    'ser.newline
    'ser.hexdump(@_fact_cal+(192*2), $20, 2, 192, 16)
    'ser.newline
    'ser.hexdump(@_fact_cal+(192*3), $20, 2, 192, 16)
    ser.strln(@"press any key")
    ser.getchar()
'4.4.4) load factory cal
    command(core.CMD_RESET_FACTORY_CAL)
    repeat mask_nr from 0 to 3
        command(core.CMD_LD_CFG_PG_FACT_CAL)
        if ( config_page_is_loaded(core.CMD_LD_CFG_PG_FACT_CAL) )
            ser.strln(@"    config page loaded")
        '_fact_cal+(0*192)+4 = _fact_cal+4 ($24)
        '_fact_cal+(1*192)+4 = _fact_cal+192+4 ($24)
        '_fact_cal+(2*192)+4 = _fact_cal+384+4 ($24)
        '_fact_cal+(3*192)+4 = _fact_cal+576+4 ($24)
        writereg(core.FACTORY_CALIBR_FIRST, 192-4, @_fact_cal+(mask_nr*192)+4)

'4.5) MEASURE cmd

    ser.str(@"measuring...")
    command(core.CMD_MEASURE)

    ser.strln(@"done")

    ser.str(@"checking app mode...")
    status := 0
    readreg(core.MODE, 1, @status)
    ser.hexs(status, 2)
    ser.newline()

var

    byte _conf[64]
    byte _image[2*64]   ' confidence value (u8), distance value (s16)

PUB measure() | tmp, int_s, sc, src, x, y, maj_x, min_x, yo, xo, zone, dest, cdest, sc_xo
'4.6)
    ser.clear()
    ser.str(@"Measuring results...")
    tmp := 0
    readreg(core.CALIBRATION_STATUS, 1, @tmp)
    ser.printf1(@"cal status = %02.2x\n\r", tmp)
    if ( tmp == $31 )
        ser.fgcolor(ser.YELLOW)
        ser.strln(@"WARNING: Factory calibration not loaded")
    elseif ( tmp == $32 )
        ser.fgcolor(ser.YELLOW)
        ser.strln(@"WARNING: Factory calibration doesn't match currently set SPAD mask")
    ser.fgcolor(ser.GREY)

    dira[INT_PIN] := 0

    repeat
        zone := 0
        repeat 4                                ' read all four subcaptures
            repeat until ( ina[INT_PIN] == 0 )  ' wait for a measurement to complete
            int_s := 0
            readreg(core.INT_STATUS, 1, @int_s) ' clear interrupt
            writereg(core.INT_STATUS, 1, int_s) '
            readreg(core.RESULT_NUMBER, 128, @_ramdump)
            sc := _ramdump[0] & $03
            case sc                             ' determine subcapture-specific X and Y offsets
                0:
                    sc_xo := 0
                    yo := 0
                1:
                    sc_xo := 4
                    yo := 0
                2:
                    sc_xo := 0
                    yo := -1
                3:
                    sc_xo := 4
                    yo := -1
            src := @_ramdump+$14                ' set source pointer to start of range data
            cdest := @_conf                     '   and per-zone confidence data
            zone := 1
            x := 0                              ' init x,y coords to lower-left (reference
            y := 7                              '   figure 37 in the datasheet)
            xo := 0
            ser.pos_xy(0, 3)
            ser.printf2(@"sub-capture: %d  result #: %2.2d", sc, (_ramdump[0] >> 2) & $3f)

            { construct an "image" frame from a composite of the four subcaptures }
            repeat 16
                ' set dest pointer to lower-left of image array
                dest := @_image + ((y+yo) * 16) + (x+xo+sc_xo)
                bytemove(cdest++, src++, 1)     ' copy zone confidence value
                bytemove(dest, src, 2)          ' copy range data
                src += 2                        ' point to next zone
                x += 8                          ' go to next x major offset
                if ( x > 8 )                    '   wrap around
                    x := 0
                    y := y - 2                  ' go up two rows
                    if ( y < 1 )                '   wrap around
                        y := 7
                zone++
                if ( zone > 8 )
                    src += 3                    ' skip zone 9; not used
                    xo := 2
                if ( zone > 17 )
                    zone := 0
                    xo := 0
        ser.pos_xy(0, 5)
        ser.hexdump_noascii(@_image, 0, 2, 128, 16, 2)


pub active_range(r)
' Set active range (section 4.2.3)
'   r:
'       SHORT_RANGE_ACC ($6e)
'       LONG_RANGE_ACC ($6f)
'   Returns:
'       0 if successful
'       -1 for invalid values
    if ( (r == core.SHORT_RANGE_ACC) or (r == core.LONG_RANGE_ACC) )
        command(r)
        return 0
    else
        return -1


pub config_page_is_loaded(cid): s | tmp
' Check if a particular config page is loaded
'   cid: configuration page id
'   Returns: TRUE (-1) or FALSE (0)
    tmp := 0
    readreg(core.CONFIG_RESULT, 4, @tmp)

    { verify the config page was loaded by checking the first few bytes: [cid][xx][$bc][$00] }
    if ( (tmp.byte[0] == cid) and (tmp.word[1] == $00_bc) )
        return true
    else
        return false


PUB bl_command(cmd, len, ptr_args) | ck
' Execute bootloader command
    i2c.start()
    i2c.write(SLAVE_WR)
    i2c.write(core.BL_CMD_STAT)
    i2c.write(cmd)
    i2c.write(len)                              ' BL_SIZE
    ck := cmd + len
    if (len > 4)
        i2c.wrblock_lsbf(ptr_args, len)
        ck += sum_blk(ptr_args, len)
    elseif (len == 0)
        ck := ck
    elseif (len < 4)
        i2c.wrblock_lsbf(@ptr_args, len)
        ck += sum_blk(@ptr_args, len)

    ser.printf1(@"sum_blk: %02.2x\n\r", ck ^ $ff)
    i2c.write(ck.byte[0] ^ $ff)  ' cksum
    i2c.stop()


PUB sum_blk(ptr_data, len): ck | tmp
' Sum a block of data
    ck := 0
    repeat tmp from 0 to len-1
        ck += byte[ptr_data][tmp]


PUB wr_command(cmd): status
' Execute application command
'   cmd: command to execute
'   Returns:
'       $00: success
'       $01: command accepted
'       $02..$0f: error
    i2c.start()
    i2c.write(SLAVE_WR)
    i2c.write(core.CMD_STAT)
    i2c.write(cmd)
    i2c.stop()

    repeat
        status := 0
        readreg(core.CMD_STAT, 1, @status)
    until ( (status => $00) and (status =< $0f) )


PUB command(cmd): status
' Send command
'   Return on success
'   Display error message and halt on error
    status := wr_command(cmd)
    if ( status > $01 )
        ser.printf1(@"\033[0;31merror %02.2x\033[m\n\r", status)
        repeat

PUB readreg(reg_nr, nr_bytes, ptr_buff)
' Read register(s) from the device
'   reg_nr: (starting) register number
'   nr_bytes: number of bytes to read
'   ptr_buff: pointer to buffer to read data to
    i2c.start()
    i2c.write(SLAVE_WR)
    i2c.write(reg_nr)
    i2c.start()
    i2c.write(SLAVE_RD)
    i2c.rdblock_lsbf(ptr_buff, nr_bytes, i2c.NAK)
    i2c.stop()


PUB bl_wait_rdy() | tries, ena
' Wait for bootloader to signal ready
    ser.str(@"polling bootloader for readiness...")
    { poll bootloader - READY? }
    tries := 0
    repeat
        ena := 0
        readreg(core.BL_CMD_STAT, 3, @ena)
        ++tries
    until (ena == $ff_00_00)
    ser.printf1(@"ready after %d tries\n\r", tries)


PUB writereg(reg_nr, nr_bytes, ptr_buff)
' Write to device register(s)
'   reg_nr: (starting) register number
'   nr_bytes: number of bytes to write
'   ptr_buff: if nr_bytes is 1..4, the value(s) to write
'             if nr_bytes is >4, a pointer to the buffer of data to write
    i2c.start()
    i2c.write(SLAVE_WR)
    i2c.write(reg_nr)
    if ((nr_bytes => 1) and (nr_bytes =< 4))
        i2c.wrblock_lsbf(@ptr_buff, nr_bytes)
    else
        i2c.wrblock_lsbf(ptr_buff, nr_bytes)    ' indirect
    i2c.stop()

' NOTE: The TMF8828 firmware image (tmf8828_image.c) must be extracted from the AMS driver,
'   translated to the equivalent spin source and #included below.
' See https://github.com/ams-OSRAM-Group/tmf8820_21_28_driver_arduino/tree/main/how_to_generate_image_from_hex
' TODO: make this easier
#include "tmf8828_image.spinh"