// Part 2.1: CAN reception with a filter. Only frames with id RX_CAN_ID are
// accepted. The driver puts them in a message queue from interrupt context,
// and the main loop takes them out and prints them.
// Built only with the can_rx_test environment (-D CAN_RX_TEST=1).
//
// Test from the Pi:
//   cansend can0 321#DEADBEEF   -> printed by the Nucleo
//   cansend can0 322#DEADBEEF   -> rejected by the filter, nothing printed

#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/can.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#define CANBUS_NODE  DT_CHOSEN(zephyr_canbus)
#define CAN_BITRATE  125000  // Must match the bitrate of can0 on the Pi
#define RX_CAN_ID    0x321   // The only id this node accepts

// Queue for up to 10 received frames. The CAN driver fills it in the
// receive interrupt; nothing else runs in interrupt context.
CAN_MSGQ_DEFINE(rx_msgq, 10);

void can_rx_test_main(void)
{
    const struct device *dev = DEVICE_DT_GET(CANBUS_NODE);

    if (!device_is_ready(dev)) {
        printk("CAN device not ready\n");
        return;
    }

    int err = can_set_bitrate(dev, CAN_BITRATE);
    if (err != 0) {
        printk("Error setting CAN bitrate (err %d)\n", err);
        return;
    }

    // Accept only RX_CAN_ID: all 11 bits of the id must match the mask.
    const struct can_filter filter = {
        .id = RX_CAN_ID,
        .mask = CAN_STD_ID_MASK,
#ifdef CAN_FILTER_DATA
        // Older Zephyr versions must be told to match data frames.
        .flags = CAN_FILTER_DATA,
#else
        .flags = 0,
#endif
    };

    // Returns a filter id (>= 0) on success.
    int filter_id = can_add_rx_filter_msgq(dev, &rx_msgq, &filter);
    if (filter_id < 0) {
        printk("Error adding RX filter (err %d)\n", filter_id);
        return;
    }

    err = can_start(dev);
    if (err != 0) {
        printk("Error starting CAN controller (err %d)\n", err);
        return;
    }

    printk("CAN RX test: %d bit/s, accepting id 0x%03X\n",
           CAN_BITRATE, RX_CAN_ID);

    struct can_frame frame;

    while (1) {
        // Sleeps until a frame is in the queue, then processes it here in
        // the main thread, where printing is allowed.
        k_msgq_get(&rx_msgq, &frame, K_FOREVER);

        printk("Received id 0x%03X [%d]:", frame.id, frame.dlc);
        for (int i = 0; i < can_dlc_to_bytes(frame.dlc); i++) {
            printk(" %02X", frame.data[i]);
        }
        printk("\n");
    }
}