399 lines
11 KiB
C
399 lines
11 KiB
C
|
/***************************************************************************
|
||
|
|
* __________ __ ___.
|
||
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
||
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
||
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
||
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
||
|
|
* \/ \/ \/ \/ \/
|
||
|
|
* $Id$
|
||
|
|
*
|
||
|
|
* Copyright (C) 2021 Aidan MacDonald
|
||
|
|
*
|
||
|
|
* This program is free software; you can redistribute it and/or
|
||
|
|
* modify it under the terms of the GNU General Public License
|
||
|
|
* as published by the Free Software Foundation; either version 2
|
||
|
|
* of the License, or (at your option) any later version.
|
||
|
|
*
|
||
|
|
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
|
||
|
|
* KIND, either express or implied.
|
||
|
|
*
|
||
|
|
****************************************************************************/
|
||
|
|
|
||
|
|
#include "i2c-async.h"
|
||
|
|
#include "config.h"
|
||
|
|
#include "system.h"
|
||
|
|
#include "kernel.h"
|
||
|
|
|
||
|
|
/* To decide on the queue size, typically you should use the formula:
|
||
|
|
*
|
||
|
|
* queue size = max { D_1, ..., D_n } * max { T_1, ..., T_m }
|
||
|
|
*
|
||
|
|
* where
|
||
|
|
*
|
||
|
|
* n = number of busses
|
||
|
|
* m = number of devices across all busses
|
||
|
|
* D_i = number of devices on bus i
|
||
|
|
* T_j = number of queued transactions needed for device j
|
||
|
|
*
|
||
|
|
* The idea is to ensure nobody waits for a queue slot, but keep the queue
|
||
|
|
* size to a minimum so that queue management features don't take too long.
|
||
|
|
* (They have to iterate over the entire queue with IRQs disabled, so...)
|
||
|
|
*
|
||
|
|
* If you don't use the asychronous features then waiting for a queue slot
|
||
|
|
* is a moot point since you'll be blocking until completion anyway; in that
|
||
|
|
* case feel free to use single-entry queues.
|
||
|
|
*
|
||
|
|
* Note that each bus gets the same sized queue. If this isn't good for
|
||
|
|
* your target for whatever reason, it might be worth implementing per-bus
|
||
|
|
* queue sizes here.
|
||
|
|
*/
|
||
|
|
#if !defined(I2C_ASYNC_BUS_COUNT) || !defined(I2C_ASYNC_QUEUE_SIZE)
|
||
|
|
# error "Need to add #defines for i2c-async frontend"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#if I2C_ASYNC_QUEUE_SIZE < 1
|
||
|
|
# error "i2c-async queue size must be >= 1"
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Singly-linked list for queuing up transactions */
|
||
|
|
typedef struct i2c_async_txn {
|
||
|
|
struct i2c_async_txn* next;
|
||
|
|
i2c_descriptor* desc;
|
||
|
|
int cookie;
|
||
|
|
} i2c_async_txn;
|
||
|
|
|
||
|
|
typedef struct i2c_async_bus {
|
||
|
|
/* Head/tail of transaction queue. The head always points to the
|
||
|
|
* currently running transaction, or NULL if the bus is idle.
|
||
|
|
*/
|
||
|
|
i2c_async_txn* head;
|
||
|
|
i2c_async_txn* tail;
|
||
|
|
|
||
|
|
/* Head of a free list used to allocate nodes. */
|
||
|
|
i2c_async_txn* free;
|
||
|
|
|
||
|
|
/* Next unallocated cookie value */
|
||
|
|
int nextcookie;
|
||
|
|
|
||
|
|
/* Semaphore for reserving a node in the free list. Nodes can only be
|
||
|
|
* allocated after a successful semaphore_wait(), and after being freed
|
||
|
|
* semaphore_release() must be called.
|
||
|
|
*/
|
||
|
|
struct semaphore sema;
|
||
|
|
|
||
|
|
#ifdef HAVE_CORELOCK_OBJECT
|
||
|
|
/* Corelock is required for multi-core CPUs */
|
||
|
|
struct corelock cl;
|
||
|
|
#endif
|
||
|
|
} i2c_async_bus;
|
||
|
|
|
||
|
|
static i2c_async_txn i2c_async_txns[I2C_ASYNC_BUS_COUNT][I2C_ASYNC_QUEUE_SIZE];
|
||
|
|
static i2c_async_bus i2c_async_busses[I2C_ASYNC_BUS_COUNT];
|
||
|
|
|
||
|
|
static i2c_async_txn* i2c_async_popfree(i2c_async_txn** head)
|
||
|
|
{
|
||
|
|
i2c_async_txn* node = *head;
|
||
|
|
*head = node->next;
|
||
|
|
return node;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void i2c_async_pushfree(i2c_async_txn** head, i2c_async_txn* node)
|
||
|
|
{
|
||
|
|
node->next = *head;
|
||
|
|
*head = node;
|
||
|
|
}
|
||
|
|
|
||
|
|
static i2c_async_txn* i2c_async_pool_init(i2c_async_txn* array, int count)
|
||
|
|
{
|
||
|
|
/* Populate forward pointers */
|
||
|
|
for(int i = 0; i < count - 1; ++i)
|
||
|
|
array[i].next = &array[i+1];
|
||
|
|
|
||
|
|
/* Last pointer is NULL */
|
||
|
|
array[count - 1].next = NULL;
|
||
|
|
|
||
|
|
/* Return head of pool */
|
||
|
|
return &array[0];
|
||
|
|
}
|
||
|
|
|
||
|
|
static void i2c_async_bus_init(int busnr)
|
||
|
|
{
|
||
|
|
const int q_size = I2C_ASYNC_QUEUE_SIZE;
|
||
|
|
|
||
|
|
i2c_async_bus* bus = &i2c_async_busses[busnr];
|
||
|
|
bus->head = bus->tail = NULL;
|
||
|
|
bus->free = i2c_async_pool_init(i2c_async_txns[busnr], q_size);
|
||
|
|
bus->nextcookie = 1;
|
||
|
|
semaphore_init(&bus->sema, q_size, q_size);
|
||
|
|
corelock_init(&bus->cl);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Add a node to the end of the transaction queue */
|
||
|
|
static void i2c_async_bus_enqueue(i2c_async_bus* bus, i2c_async_txn* node)
|
||
|
|
{
|
||
|
|
node->next = NULL;
|
||
|
|
if(bus->head == NULL)
|
||
|
|
bus->head = bus->tail = node;
|
||
|
|
else {
|
||
|
|
bus->tail->next = node;
|
||
|
|
bus->tail = node;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Helper function called to run descriptor completion tasks */
|
||
|
|
static void i2c_async_bus_complete_desc(i2c_async_bus* bus, int status)
|
||
|
|
{
|
||
|
|
i2c_descriptor* d = bus->head->desc;
|
||
|
|
if(d->callback)
|
||
|
|
d->callback(status, d);
|
||
|
|
|
||
|
|
bus->head->desc = d->next;
|
||
|
|
}
|
||
|
|
|
||
|
|
void __i2c_async_complete_callback(int busnr, int status)
|
||
|
|
{
|
||
|
|
i2c_async_bus* bus = &i2c_async_busses[busnr];
|
||
|
|
corelock_lock(&bus->cl);
|
||
|
|
|
||
|
|
i2c_async_bus_complete_desc(bus, status);
|
||
|
|
if(status != I2C_STATUS_OK) {
|
||
|
|
/* Skip remainder of transaction after an error */
|
||
|
|
while(bus->head->desc)
|
||
|
|
i2c_async_bus_complete_desc(bus, I2C_STATUS_SKIPPED);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Dequeue next transaction if we finished the current one */
|
||
|
|
if(!bus->head->desc) {
|
||
|
|
i2c_async_txn* node = bus->head;
|
||
|
|
bus->head = node->next;
|
||
|
|
i2c_async_pushfree(&bus->free, node);
|
||
|
|
semaphore_release(&bus->sema);
|
||
|
|
}
|
||
|
|
|
||
|
|
if(bus->head) {
|
||
|
|
/* Submit the next descriptor */
|
||
|
|
__i2c_async_submit(busnr, bus->head->desc);
|
||
|
|
} else {
|
||
|
|
/* Fixup tail after last transaction */
|
||
|
|
bus->tail = NULL;
|
||
|
|
}
|
||
|
|
|
||
|
|
corelock_unlock(&bus->cl);
|
||
|
|
}
|
||
|
|
|
||
|
|
void __i2c_async_init(void)
|
||
|
|
{
|
||
|
|
for(int i = 0; i < I2C_ASYNC_BUS_COUNT; ++i)
|
||
|
|
i2c_async_bus_init(i);
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_async_queue(int busnr, int timeout, int q_mode,
|
||
|
|
int cookie, i2c_descriptor* desc)
|
||
|
|
{
|
||
|
|
i2c_async_txn* node;
|
||
|
|
i2c_async_bus* bus = &i2c_async_busses[busnr];
|
||
|
|
int rc = I2C_RC_OK;
|
||
|
|
|
||
|
|
int irq = disable_irq_save();
|
||
|
|
corelock_lock(&bus->cl);
|
||
|
|
|
||
|
|
/* Scan the queue unless q_mode is a simple ADD */
|
||
|
|
if(q_mode != I2C_Q_ADD) {
|
||
|
|
for(node = bus->head; node != NULL; node = node->next) {
|
||
|
|
if(node->cookie == cookie) {
|
||
|
|
if(q_mode == I2C_Q_REPLACE && node != bus->head)
|
||
|
|
node->desc = desc;
|
||
|
|
else
|
||
|
|
rc = I2C_RC_NOTADDED;
|
||
|
|
goto _exit;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Try to claim a queue node without blocking if we can */
|
||
|
|
if(semaphore_wait(&bus->sema, TIMEOUT_NOBLOCK) != OBJ_WAIT_SUCCEEDED) {
|
||
|
|
/* Bail out now if caller doesn't want to wait */
|
||
|
|
if(timeout == TIMEOUT_NOBLOCK) {
|
||
|
|
rc = I2C_RC_BUSY;
|
||
|
|
goto _exit;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Wait on the semaphore */
|
||
|
|
corelock_unlock(&bus->cl);
|
||
|
|
restore_irq(irq);
|
||
|
|
if(semaphore_wait(&bus->sema, timeout) == OBJ_WAIT_TIMEDOUT)
|
||
|
|
return I2C_RC_BUSY;
|
||
|
|
|
||
|
|
/* Got a node; re-lock */
|
||
|
|
irq = disable_irq_save();
|
||
|
|
corelock_lock(&bus->cl);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Alloc the node and push it to queue */
|
||
|
|
node = i2c_async_popfree(&bus->free);
|
||
|
|
node->desc = desc;
|
||
|
|
node->cookie = cookie;
|
||
|
|
i2c_async_bus_enqueue(bus, node);
|
||
|
|
|
||
|
|
/* Start the first descriptor if the bus is idle */
|
||
|
|
if(node == bus->head)
|
||
|
|
__i2c_async_submit(busnr, desc);
|
||
|
|
|
||
|
|
_exit:
|
||
|
|
corelock_unlock(&bus->cl);
|
||
|
|
restore_irq(irq);
|
||
|
|
return rc;
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_async_cancel(int busnr, int cookie)
|
||
|
|
{
|
||
|
|
i2c_async_bus* bus = &i2c_async_busses[busnr];
|
||
|
|
int rc = I2C_RC_NOTFOUND;
|
||
|
|
|
||
|
|
int irq = disable_irq_save();
|
||
|
|
corelock_lock(&bus->cl);
|
||
|
|
|
||
|
|
/* Bail if queue is empty */
|
||
|
|
if(!bus->head)
|
||
|
|
goto _exit;
|
||
|
|
|
||
|
|
/* Check the running transaction for a match */
|
||
|
|
if(bus->head->cookie == cookie) {
|
||
|
|
rc = I2C_RC_BUSY;
|
||
|
|
goto _exit;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Walk the queue, starting after the head */
|
||
|
|
i2c_async_txn* prev = bus->head;
|
||
|
|
i2c_async_txn* node = prev->next;
|
||
|
|
while(node) {
|
||
|
|
if(node->cookie == cookie) {
|
||
|
|
prev->next = node->next;
|
||
|
|
rc = I2C_RC_OK;
|
||
|
|
goto _exit;
|
||
|
|
}
|
||
|
|
|
||
|
|
prev = node;
|
||
|
|
node = node->next;
|
||
|
|
}
|
||
|
|
|
||
|
|
_exit:
|
||
|
|
corelock_unlock(&bus->cl);
|
||
|
|
restore_irq(irq);
|
||
|
|
return rc;
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_async_reserve_cookies(int busnr, int count)
|
||
|
|
{
|
||
|
|
i2c_async_bus* bus = &i2c_async_busses[busnr];
|
||
|
|
int ret = bus->nextcookie;
|
||
|
|
bus->nextcookie += count;
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void i2c_sync_callback(int status, i2c_descriptor* d)
|
||
|
|
{
|
||
|
|
struct semaphore* sem = (struct semaphore*)d->arg;
|
||
|
|
d->arg = status;
|
||
|
|
semaphore_release(sem);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void i2c_reg_modify1_callback(int status, i2c_descriptor* d)
|
||
|
|
{
|
||
|
|
if(status == I2C_STATUS_OK) {
|
||
|
|
uint8_t* buf = (uint8_t*)d->buffer[1];
|
||
|
|
uint8_t val = *buf;
|
||
|
|
*buf = (val & ~(d->arg >> 8)) | (d->arg & 0xff);
|
||
|
|
d->arg = val;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_reg_write(int bus, uint8_t addr, uint8_t reg,
|
||
|
|
int count, const uint8_t* buf)
|
||
|
|
{
|
||
|
|
struct semaphore sem;
|
||
|
|
semaphore_init(&sem, 1, 0);
|
||
|
|
|
||
|
|
i2c_descriptor desc;
|
||
|
|
desc.slave_addr = addr;
|
||
|
|
desc.bus_cond = I2C_START | I2C_STOP;
|
||
|
|
desc.tran_mode = I2C_WRITE;
|
||
|
|
desc.buffer[0] = ®
|
||
|
|
desc.count[0] = 1;
|
||
|
|
desc.buffer[1] = (uint8_t*)buf;
|
||
|
|
desc.count[1] = count;
|
||
|
|
desc.callback = &i2c_sync_callback;
|
||
|
|
desc.arg = (intptr_t)&sem;
|
||
|
|
desc.next = NULL;
|
||
|
|
|
||
|
|
i2c_async_queue(bus, TIMEOUT_BLOCK, I2C_Q_ADD, 0, &desc);
|
||
|
|
semaphore_wait(&sem, TIMEOUT_BLOCK);
|
||
|
|
|
||
|
|
return desc.arg;
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_reg_read(int bus, uint8_t addr, uint8_t reg,
|
||
|
|
int count, uint8_t* buf)
|
||
|
|
{
|
||
|
|
struct semaphore sem;
|
||
|
|
semaphore_init(&sem, 1, 0);
|
||
|
|
|
||
|
|
i2c_descriptor desc;
|
||
|
|
desc.slave_addr = addr;
|
||
|
|
desc.bus_cond = I2C_START | I2C_STOP;
|
||
|
|
desc.tran_mode = I2C_READ;
|
||
|
|
desc.buffer[0] = ®
|
||
|
|
desc.count[0] = 1;
|
||
|
|
desc.buffer[1] = buf;
|
||
|
|
desc.count[1] = count;
|
||
|
|
desc.callback = &i2c_sync_callback;
|
||
|
|
desc.arg = (intptr_t)&sem;
|
||
|
|
desc.next = NULL;
|
||
|
|
|
||
|
|
i2c_async_queue(bus, TIMEOUT_BLOCK, I2C_Q_ADD, 0, &desc);
|
||
|
|
semaphore_wait(&sem, TIMEOUT_BLOCK);
|
||
|
|
|
||
|
|
return desc.arg;
|
||
|
|
}
|
||
|
|
|
||
|
|
int i2c_reg_modify1(int bus, uint8_t addr, uint8_t reg,
|
||
|
|
uint8_t clr, uint8_t set, uint8_t* val)
|
||
|
|
{
|
||
|
|
struct semaphore sem;
|
||
|
|
semaphore_init(&sem, 1, 0);
|
||
|
|
|
||
|
|
uint8_t buf[2];
|
||
|
|
buf[0] = reg;
|
||
|
|
|
||
|
|
i2c_descriptor desc[2];
|
||
|
|
desc[0].slave_addr = addr;
|
||
|
|
desc[0].bus_cond = I2C_START | I2C_STOP;
|
||
|
|
desc[0].tran_mode = I2C_READ;
|
||
|
|
desc[0].buffer[0] = &buf[0];
|
||
|
|
desc[0].count[0] = 1;
|
||
|
|
desc[0].buffer[1] = &buf[1];
|
||
|
|
desc[0].count[1] = 1;
|
||
|
|
desc[0].callback = &i2c_reg_modify1_callback;
|
||
|
|
desc[0].arg = (clr << 8) | set;
|
||
|
|
desc[0].next = &desc[1];
|
||
|
|
|
||
|
|
desc[1].slave_addr = addr;
|
||
|
|
desc[1].bus_cond = I2C_START | I2C_STOP;
|
||
|
|
desc[1].tran_mode = I2C_WRITE;
|
||
|
|
desc[1].buffer[0] = &buf[0];
|
||
|
|
desc[1].count[0] = 2;
|
||
|
|
desc[1].buffer[1] = NULL;
|
||
|
|
desc[1].count[1] = 0;
|
||
|
|
desc[1].callback = &i2c_sync_callback;
|
||
|
|
desc[1].arg = (intptr_t)&sem;
|
||
|
|
desc[1].next = NULL;
|
||
|
|
|
||
|
|
i2c_async_queue(bus, TIMEOUT_BLOCK, I2C_Q_ADD, 0, &desc[0]);
|
||
|
|
semaphore_wait(&sem, TIMEOUT_BLOCK);
|
||
|
|
|
||
|
|
if(val && desc[1].arg == I2C_STATUS_OK)
|
||
|
|
*val = desc[0].arg;
|
||
|
|
|
||
|
|
return desc[1].arg;
|
||
|
|
}
|