forward commands from laptop to battery
This commit is contained in:
+2
-1
@@ -35,7 +35,7 @@ pico_sdk_init()
|
||||
|
||||
# Add executable. Default name is the project name, version 0.1
|
||||
|
||||
add_executable(BattMITM_2.0 main.c smbus.c )
|
||||
add_executable(BattMITM_2.0 main.c smbus.c static_queue.c mitm.c status.c )
|
||||
|
||||
pico_set_program_name(BattMITM_2.0 "BattMITM_2.0")
|
||||
pico_set_program_version(BattMITM_2.0 "0.1")
|
||||
@@ -44,6 +44,7 @@ pico_set_program_version(BattMITM_2.0 "0.1")
|
||||
target_link_libraries(BattMITM_2.0
|
||||
pico_stdlib
|
||||
hardware_i2c
|
||||
pico_i2c_slave
|
||||
)
|
||||
|
||||
# Add the standard include files to the build
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
// use on-board led
|
||||
#define STATUS_LED_PIN 25
|
||||
|
||||
// i2c connected to battery (as master)
|
||||
#define BATT_I2C i2c0
|
||||
#define BATT_I2C_SDA_PIN PICO_DEFAULT_I2C_SDA_PIN // 4
|
||||
#define BATT_I2C_SCL_PIN PICO_DEFAULT_I2C_SCL_PIN // 5
|
||||
#define BATT_I2C_PULL_UP true // disable this if you have an external resistor
|
||||
|
||||
#define BATT_I2C_ADDR 0x0b
|
||||
#define BATT_I2C_BAUD 32000
|
||||
|
||||
// i2c connected to laptop (as slave)
|
||||
#define LAPTOP_I2C i2c1
|
||||
#define LAPTOP_I2C_SDA_PIN 2
|
||||
#define LAPTOP_I2C_SCL_PIN 3
|
||||
#define LAPTOP_I2C_PULL_UP false // enable this if your laptop smbus is broken ig
|
||||
|
||||
#define LAPTOP_I2C_ADDR BATT_I2C_ADDR // use the same address
|
||||
#define LAPTOP_I2C_BAUD BATT_I2C_BAUD // use the same baud (for now)
|
||||
|
||||
|
||||
@@ -21,31 +21,12 @@
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "mitm.h"
|
||||
#include "smbus.h"
|
||||
#include "status.h"
|
||||
#include "config.h"
|
||||
#include <stdio.h>
|
||||
#include "pico/stdlib.h"
|
||||
#include "smbus.h"
|
||||
|
||||
|
||||
// use on-board led
|
||||
#define STATUS_LED_PIN 25
|
||||
|
||||
|
||||
#define BATT_I2C i2c0
|
||||
#define BATT_I2C_SDA_PIN PICO_DEFAULT_I2C_SDA_PIN
|
||||
#define BATT_I2C_SCL_PIN PICO_DEFAULT_I2C_SCL_PIN
|
||||
|
||||
#define BATT_I2C_ADDR 0x0b
|
||||
#define BATT_I2C_BAUD 32000
|
||||
|
||||
#define BATT_CMD_VOLTAGE 0x09
|
||||
#define BATT_CMD_SERIAL 0x1c
|
||||
#define BATT_CMD_MANUFACTURER_ACCESS 0x00
|
||||
#define BATT_CMD_VENDOR 0x20
|
||||
|
||||
|
||||
void status_led(int state) {
|
||||
gpio_put(STATUS_LED_PIN, state);
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
@@ -55,48 +36,38 @@ int main() {
|
||||
|
||||
printf("\n\n\n======= init =======\n\n\n");
|
||||
|
||||
gpio_init(STATUS_LED_PIN);
|
||||
gpio_set_dir(STATUS_LED_PIN, GPIO_OUT);
|
||||
i2c_dev_t* bms = get_bms_dev();
|
||||
i2c_dev_t* laptop = get_laptop_dev();
|
||||
|
||||
init_status();
|
||||
|
||||
i2c_init(BATT_I2C, BATT_I2C_BAUD);
|
||||
// init batt i2c
|
||||
gpio_set_function(BATT_I2C_SDA_PIN, GPIO_FUNC_I2C);
|
||||
gpio_set_function(BATT_I2C_SCL_PIN, GPIO_FUNC_I2C);
|
||||
gpio_pull_up(BATT_I2C_SDA_PIN);
|
||||
gpio_pull_up(BATT_I2C_SCL_PIN);
|
||||
i2c_init(BATT_I2C, BATT_I2C_BAUD);
|
||||
if (BATT_I2C_PULL_UP) {
|
||||
gpio_pull_up(BATT_I2C_SDA_PIN);
|
||||
gpio_pull_up(BATT_I2C_SCL_PIN);
|
||||
}
|
||||
|
||||
// init laptop i2c
|
||||
gpio_set_function(LAPTOP_I2C_SDA_PIN, GPIO_FUNC_I2C);
|
||||
gpio_set_function(LAPTOP_I2C_SCL_PIN, GPIO_FUNC_I2C);
|
||||
i2c_init(LAPTOP_I2C, LAPTOP_I2C_BAUD);
|
||||
if (LAPTOP_I2C_PULL_UP) {
|
||||
gpio_pull_up(LAPTOP_I2C_SDA_PIN);
|
||||
gpio_pull_up(LAPTOP_I2C_SCL_PIN);
|
||||
}
|
||||
|
||||
i2c_dev_t bms = {i2c_default, BATT_I2C_ADDR};
|
||||
i2c_set_slave_mode(LAPTOP_I2C, true, LAPTOP_I2C_ADDR);
|
||||
|
||||
mitm_init();
|
||||
|
||||
int len;
|
||||
uint16_t voltage, serial, mf_acc;
|
||||
char vendor[32];
|
||||
|
||||
while (true) {
|
||||
sleep_ms(1000);
|
||||
|
||||
status_led(1);
|
||||
printf("reading\n");
|
||||
|
||||
|
||||
len = smbus_read_uint16(&bms, BATT_CMD_MANUFACTURER_ACCESS, &mf_acc);
|
||||
if (len > 0) printf("mf access = %04x\n", mf_acc);
|
||||
else printf("ERROR %d\n", len);
|
||||
|
||||
len = smbus_read_uint16(&bms, BATT_CMD_VOLTAGE, &voltage);
|
||||
if (len > 0) printf("voltage = %d mV\n", voltage);
|
||||
else printf("ERROR %d\n", len);
|
||||
|
||||
len = smbus_read_uint16(&bms, BATT_CMD_SERIAL, &serial);
|
||||
if (len > 0) printf("serial = %d\n", serial);
|
||||
else printf("ERROR %d\n", len);
|
||||
|
||||
len = smbus_read_text(&bms, BATT_CMD_VENDOR, &vendor[0], 32);
|
||||
if (len > 0) printf("vendor[%d] = %.*s\n", len, len, vendor);
|
||||
else printf("ERROR %d\n", len);
|
||||
|
||||
status_led(0);
|
||||
printf("done\n\n");
|
||||
|
||||
mitm_loop();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,178 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "mitm.h"
|
||||
#include "smbus.h"
|
||||
#include "status.h"
|
||||
#include "pico/i2c_slave.h"
|
||||
#include "static_queue.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
struct smbus_transfer {
|
||||
i2c_slave_event_t event;
|
||||
uint8_t data;
|
||||
};
|
||||
|
||||
typedef struct smbus_transfer smbus_transfer_t;
|
||||
|
||||
|
||||
#define MITM_QUEUE_ELEMENT_SIZE sizeof(smbus_transfer_t)
|
||||
#define MITM_QUEUE_MAX_ELEMENTS 10
|
||||
|
||||
#define MITM_CMD_BUFFER_SIZE 64
|
||||
#define MITM_REPLY_BUFFER_SIZE 64
|
||||
|
||||
|
||||
static_queue_t* mitm_transfer_queue;
|
||||
bool mitm_transfer_queue_overflow = false;
|
||||
|
||||
i2c_slave_event_t previous_event = I2C_SLAVE_FINISH;
|
||||
|
||||
uint8_t mitm_cmd_buffer[MITM_CMD_BUFFER_SIZE];
|
||||
size_t mitm_cmd_buffer_index = 0;
|
||||
|
||||
uint8_t mitm_reply_buffer[MITM_REPLY_BUFFER_SIZE];
|
||||
size_t mitm_reply_buffer_index = 0;
|
||||
|
||||
|
||||
void mitm_laptop_irq_handler(i2c_inst_t* i2c, i2c_slave_event_t event) {
|
||||
smbus_transfer_t* transfer = static_queue_add(mitm_transfer_queue);
|
||||
if (transfer == NULL) return; //todo
|
||||
|
||||
transfer->event = event;
|
||||
if (event == I2C_SLAVE_RECEIVE) i2c_read_raw_blocking(i2c, &transfer->data, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
int mitm_init() {
|
||||
mitm_transfer_queue = create_static_queue(MITM_QUEUE_MAX_ELEMENTS, MITM_QUEUE_ELEMENT_SIZE);
|
||||
|
||||
i2c_dev_t* laptop = get_laptop_dev();
|
||||
i2c_slave_init(laptop->i2c, laptop->address, &mitm_laptop_irq_handler);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void mitm_loop() {
|
||||
i2c_dev_t* laptop = get_laptop_dev();
|
||||
i2c_dev_t* bms = get_bms_dev();
|
||||
|
||||
smbus_transfer_t* transfer = static_queue_peek(mitm_transfer_queue);
|
||||
if (transfer == NULL) return;
|
||||
|
||||
status_led(1);
|
||||
|
||||
while (transfer != NULL) {
|
||||
|
||||
switch (transfer->event) {
|
||||
case I2C_SLAVE_RECEIVE:
|
||||
|
||||
// this should never happen
|
||||
if (previous_event == I2C_SLAVE_REQUEST) {
|
||||
printf("ERROR: no stop after read before write!!!\n");
|
||||
mitm_reply_buffer[0] = 0;
|
||||
i2c_write_raw_blocking(laptop->i2c, mitm_reply_buffer, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
if (mitm_cmd_buffer_index + 1 >= MITM_CMD_BUFFER_SIZE) {
|
||||
printf("ERROR: cmd buffer overrun!!!\n");
|
||||
break;
|
||||
}
|
||||
|
||||
// write previous byte
|
||||
if (mitm_cmd_buffer_index > 0) {
|
||||
i2c_write_burst_blocking(bms->i2c, bms->address, &mitm_cmd_buffer[mitm_cmd_buffer_index - 1], 1);
|
||||
}
|
||||
|
||||
mitm_cmd_buffer[mitm_cmd_buffer_index++] = transfer->data;
|
||||
|
||||
printf("TX 0x%02x\n", mitm_cmd_buffer[mitm_cmd_buffer_index-1]);
|
||||
|
||||
break;
|
||||
case I2C_SLAVE_REQUEST:
|
||||
|
||||
// this should never happen
|
||||
if (previous_event == I2C_SLAVE_RECEIVE) {
|
||||
printf("ERROR: no stop after write before read!!!\n");
|
||||
mitm_reply_buffer[0] = 0;
|
||||
i2c_write_raw_blocking(laptop->i2c, mitm_reply_buffer, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
if (mitm_reply_buffer_index + 1 >= MITM_REPLY_BUFFER_SIZE) {
|
||||
printf("ERROR: reply buffer overrun!!!\n");
|
||||
mitm_reply_buffer[0] = 0;
|
||||
i2c_write_raw_blocking(laptop->i2c, mitm_reply_buffer, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
// forward reply from bms
|
||||
i2c_read_burst_blocking(bms->i2c, bms->address, &mitm_reply_buffer[mitm_reply_buffer_index], 1);
|
||||
i2c_write_raw_blocking(laptop->i2c, &mitm_reply_buffer[mitm_reply_buffer_index], 1);
|
||||
|
||||
printf("RX 0x%02x\n", mitm_reply_buffer[mitm_reply_buffer_index]);
|
||||
|
||||
mitm_reply_buffer_index++;
|
||||
break;
|
||||
case I2C_SLAVE_FINISH:
|
||||
//todo: need to replace the actual interrupt if I want this to be accurate, this impl is just guessing
|
||||
if (previous_event == I2C_SLAVE_RECEIVE) {
|
||||
|
||||
bool is_cmd = mitm_cmd_buffer_index == 1;
|
||||
i2c_write_blocking(bms->i2c, bms->address, mitm_cmd_buffer + mitm_cmd_buffer_index - 1, 1, is_cmd); // cmd expects rx
|
||||
if (is_cmd) {
|
||||
printf("end of cmd\n");
|
||||
} else {
|
||||
printf("end of write (%d bytes)\n", mitm_cmd_buffer_index);
|
||||
}
|
||||
|
||||
} else if (previous_event == I2C_SLAVE_REQUEST) {
|
||||
i2c_stop_blocking(bms);
|
||||
printf("end of read (%d bytes)\n", mitm_reply_buffer_index);
|
||||
} else {
|
||||
printf("RESET\n");
|
||||
}
|
||||
|
||||
mitm_cmd_buffer_index = 0;
|
||||
mitm_reply_buffer_index = 0;
|
||||
break;
|
||||
default:
|
||||
printf("event invalid 0x%02x\n", transfer->event);
|
||||
break;
|
||||
}
|
||||
|
||||
previous_event = transfer->event;
|
||||
static_queue_pop(mitm_transfer_queue);
|
||||
transfer = static_queue_peek(mitm_transfer_queue);
|
||||
}
|
||||
|
||||
status_led(0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
int mitm_init();
|
||||
void mitm_loop();
|
||||
@@ -22,9 +22,26 @@
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "smbus.h"
|
||||
#include "config.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
struct i2c_dev;
|
||||
|
||||
|
||||
i2c_dev_t _i2c_bms_dev = {BATT_I2C, BATT_I2C_ADDR};
|
||||
i2c_dev_t _i2c_laptop_dev = {LAPTOP_I2C, LAPTOP_I2C_ADDR};
|
||||
|
||||
|
||||
i2c_dev_t* get_bms_dev() {
|
||||
return &_i2c_bms_dev;
|
||||
}
|
||||
|
||||
i2c_dev_t* get_laptop_dev() {
|
||||
return &_i2c_laptop_dev;
|
||||
}
|
||||
|
||||
|
||||
// CRC-8
|
||||
void generate_crc(uint8_t* crc, uint8_t current_byte) {
|
||||
for (int i = 0; i < 8; i++) {
|
||||
@@ -202,3 +219,4 @@ int smbus_read_text(i2c_dev_t* device, uint8_t cmd, char* result, size_t max_len
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -21,7 +21,9 @@
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "hardware/i2c.h"
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include "hardware/i2c.h"
|
||||
|
||||
#define SMBUS_ERROR_DEVICE -1
|
||||
#define SMBUS_ERROR_GENERIC -2
|
||||
@@ -35,6 +37,11 @@ struct i2c_dev {
|
||||
|
||||
typedef struct i2c_dev i2c_dev_t;
|
||||
|
||||
i2c_dev_t* get_bms_dev();
|
||||
i2c_dev_t* get_laptop_dev();
|
||||
|
||||
void i2c_stop_blocking(i2c_dev_t* device);
|
||||
|
||||
int smbus_read(i2c_dev_t* device, uint8_t cmd, uint8_t* result, size_t length);
|
||||
int smbus_read_block(i2c_dev_t* device, uint8_t cmd, uint8_t* result, size_t max_length);
|
||||
|
||||
|
||||
+120
@@ -0,0 +1,120 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "static_queue.h"
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
// statically allocated queue
|
||||
// it supports concurrently to an extent, but only if each core/interrupt only ever adds or removes from it.
|
||||
|
||||
static_queue_t* create_static_queue(size_t max_elements, size_t element_size) {
|
||||
void* data = malloc(max_elements * element_size);
|
||||
if (data == NULL) {
|
||||
printf("FATAL: static queue allocation failed\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static_queue_t* inst = malloc(sizeof(static_queue_t));
|
||||
inst->max_elements = max_elements;
|
||||
inst->element_size = element_size;
|
||||
inst->dropped = 0;
|
||||
inst->_index_end = 0;
|
||||
inst->_index_start = 0;
|
||||
inst->_data = data;
|
||||
return inst;
|
||||
}
|
||||
|
||||
void increment_index(static_queue_t* inst, size_t* index) {
|
||||
if (*index >= inst->max_elements - 1) {
|
||||
*index = 0;
|
||||
} else {
|
||||
(*index)++;
|
||||
}
|
||||
}
|
||||
|
||||
bool static_queue_is_full(static_queue_t* inst) {
|
||||
return inst->_index_start == inst->_index_end + 1 || (inst->_index_end == inst->max_elements - 1 && inst->_index_start == 0);
|
||||
}
|
||||
|
||||
bool static_queue_is_empty(static_queue_t* inst) {
|
||||
return inst->_index_start == inst->_index_end;
|
||||
}
|
||||
|
||||
void* static_queue_get_element_ptr(static_queue_t* inst, size_t index) {
|
||||
return inst->_data + index * inst->element_size;
|
||||
}
|
||||
|
||||
// returns a pointer to place data for your new queue element.
|
||||
// if the queue is full this returns a NULL pointer and increments `dropped`.
|
||||
// note that this will immediately make this memory available to be accessed (peeked/popped) from the queue.
|
||||
// if queue access may interrupt this consider adding an "initialized" byte to the element allocation
|
||||
void* static_queue_add(static_queue_t* inst) {
|
||||
if (static_queue_is_full(inst)) {
|
||||
inst->dropped++;
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void* ptr = static_queue_get_element_ptr(inst, inst->_index_end);
|
||||
increment_index(inst, &inst->_index_end);
|
||||
|
||||
return ptr;
|
||||
}
|
||||
|
||||
// removes an element off the front of the queue and returns a pointer to it.
|
||||
// returns a NULL pointer if the queue is empty.
|
||||
// note that this immediately opens that memory to get claimed by the next addition to the queue.
|
||||
// if queue additions may interrupt this consider using peek instead, then call pop when you're done.
|
||||
void* static_queue_pop(static_queue_t* inst) {
|
||||
if (static_queue_is_empty(inst)) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void* ptr = static_queue_get_element_ptr(inst, inst->_index_start);
|
||||
increment_index(inst, &inst->_index_start);
|
||||
|
||||
return ptr;
|
||||
}
|
||||
|
||||
// returns a pointer to the element at the front of the queue.
|
||||
void* static_queue_peek(static_queue_t* inst) {
|
||||
if (static_queue_is_empty(inst)) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
return static_queue_get_element_ptr(inst, inst->_index_start);
|
||||
}
|
||||
|
||||
// returns the number of elements inside of the queue
|
||||
size_t static_queue_size(static_queue_t* inst) {
|
||||
if (inst->_index_start < inst->_index_end) {
|
||||
return inst->_index_end - inst->_index_start;
|
||||
} else if (inst->_index_start > inst->_index_end) {
|
||||
return inst->max_elements - inst->_index_start + inst->_index_end;
|
||||
} else {
|
||||
return 0; // empty if both are the same
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,45 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include <stddef.h>
|
||||
|
||||
struct static_queue {
|
||||
size_t max_elements;
|
||||
size_t element_size;
|
||||
|
||||
long dropped;
|
||||
|
||||
size_t _index_end;
|
||||
size_t _index_start;
|
||||
void* _data;
|
||||
};
|
||||
|
||||
typedef struct static_queue static_queue_t;
|
||||
|
||||
static_queue_t* create_static_queue(size_t max_elements, size_t element_size);
|
||||
|
||||
void* static_queue_add(static_queue_t* inst);
|
||||
void* static_queue_pop(static_queue_t* inst);
|
||||
void* static_queue_peek(static_queue_t* inst);
|
||||
size_t static_queue_size(static_queue_t* inst);
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
#include "status.h"
|
||||
#include "config.h"
|
||||
#include "hardware/gpio.h"
|
||||
|
||||
void init_status() {
|
||||
gpio_init(STATUS_LED_PIN);
|
||||
gpio_set_dir(STATUS_LED_PIN, GPIO_OUT);
|
||||
}
|
||||
|
||||
// blinking leds always make things look cooler than they are
|
||||
void status_led(int state) {
|
||||
gpio_put(STATUS_LED_PIN, state);
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
/**
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2025 Benjamin Wiegand
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||||
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
||||
IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
void init_status();
|
||||
void status_led(int state);
|
||||
Reference in New Issue
Block a user