qemu-xteink/hw/misc/esp32c3_wifi.c

#include "qemu/osdep.h"
#include "qemu/log.h"
#include "qemu/error-report.h"
#include "qemu/guest-random.h"
#include "qapi/error.h"
#include "sysemu/sysemu.h"
#include "hw/hw.h"
#include "hw/irq.h"
#include "hw/sysbus.h"
#include "hw/misc/esp32c3_wifi.h"
#include "exec/address-spaces.h"
#include "esp32_wlan_packet.h"
#include "hw/qdev-properties.h"
#include "hw/misc/esp32c3_reg.h"

#define DEBUG 0

extern access_point_info access_points[];
extern int nb_aps;

static uint64_t esp32C3_wifi_read(void *opaque, hwaddr addr, unsigned int size)
{
    
    Esp32WifiState *s = ESP32_WIFI(opaque);
    uint32_t r = s->mem[addr/4];
    
    switch(addr) {
        case A_C3_WIFI_DMA_IN_STATUS:
            r = 0;
            break;
        case A_C3_WIFI_DMA_INT_STATUS:
        case A_C3_WIFI_DMA_INT_CLR:
            r = s->raw_interrupt;
            break;
        case A_C3_WIFI_STATUS:
        case A_C3_WIFI_DMA_OUT_STATUS:
            r = 1;
            break;
        default:
            break;
    }

    if (DEBUG) printf("esp32C3_wifi_read  0x%04lx= 0x%08x\n",(unsigned long) addr,r);

    return r;
}
static void set_interrupt(Esp32WifiState *s,int e) {
    s->raw_interrupt |= e;
    qemu_set_irq(s->irq, 1);
}

void Esp32_WLAN_frame_delivered(Esp32WifiState *s){
    s->raw_interrupt |= 0x80;
    qemu_set_irq(s->irq, 1);
}

static void esp32C3_wifi_write(void *opaque, hwaddr addr, uint64_t value,
                                 unsigned int size) {
    Esp32WifiState *s = ESP32_WIFI(opaque);
    if(DEBUG) printf("esp32C3_wifi_write 0x%04lx= 0x%08lx\n",(unsigned long) addr, (unsigned long) value);

    switch (addr) {
        case A_C3_WIFI_DMA_INLINK: {
            uint32_t offset = value & 0xfffff;
            s->dma_inlink_address = offset ? 0x3fc00000 | offset : 0;
            s->mem[0x90 / sizeof(uint32_t)] = s->dma_inlink_address;
            break;
        }
        case A_C3_WIFI_DMA_INT_CLR:
            s->raw_interrupt &= ~value;
            if (s->raw_interrupt == 0) {
                qemu_set_irq(s->irq, 0);
            }
            break;
        case A_C3_WIFI_DMA_OUTLINK:
            if ((value & 0xc0000000) && (value & 0xfffff)) {
                mac80211_frame frame;
                dma_list_item item;
                unsigned memaddr = 0x3fc00000 | (value & 0xfffff);
                address_space_read(&address_space_memory, memaddr,
                                   MEMTXATTRS_UNSPECIFIED, &item, sizeof(item));
                if (item.length > sizeof(frame)) {
                    break;
                }
                address_space_read(&address_space_memory, item.address,
                                   MEMTXATTRS_UNSPECIFIED, &frame, item.length);
                frame.frame_length = item.length;
                frame.next_frame = NULL;
                Esp32_WLAN_handle_frame(s, &frame);
            }
            break;
        default:
            break;
    }
    s->mem[addr/4]=value;
}

static int match_mac_address(uint8_t *a1,uint8_t *a2) {
    if(!memcmp(a1,a2,6)) return 1;
    if(!memcmp(a1,BROADCAST,6)) return 1;
    return 0;
}
// frame from ap to esp32
void Esp32_sendFrame(Esp32WifiState *s, mac80211_frame *frame,int length, int signal_strength) {
    int packet_channel = esp32_wifi_channel;

    if (s->dma_inlink_address == 0) {
        return;
    }
    if (frame->frame_control.type == IEEE80211_TYPE_MGT &&
        frame->frame_control.sub_type == IEEE80211_TYPE_MGT_SUBTYPE_BEACON) {
        int pos = 12;
        int data_length = length - IEEE80211_HEADER_SIZE;

        while (pos + 2 <= data_length) {
            uint8_t tag = frame->data_and_fcs[pos];
            uint8_t tag_length = frame->data_and_fcs[pos + 1];

            if (tag == IEEE80211_BEACON_PARAM_CHANNEL && tag_length) {
                packet_channel = frame->data_and_fcs[pos + 2];
                break;
            }
            pos += 2 + tag_length;
        }
    }
    if (packet_channel == 0) {
        const uint8_t *address = frame->frame_control.type == IEEE80211_TYPE_DATA
            ? frame->bssid_address : frame->source_address;

        for (int i = 0; i < nb_aps; i++) {
            if (memcmp(access_points[i].mac_address, address, 6) == 0) {
                packet_channel = access_points[i].channel;
                break;
            }
        }
    }
    uint8_t header[sizeof(wifi_pkt_rx_ctrl_c3_t)+length];
    memset(header,0,sizeof(header));
    wifi_pkt_rx_ctrl_c3_t *pkt=(wifi_pkt_rx_ctrl_c3_t *)header;
    *pkt=(wifi_pkt_rx_ctrl_c3_t){
        .rssi=(signal_strength+(rand()%10)+96),
        .rate=11,
        .sig_len=length,
        .sig_len_copy=length,
        .legacy_length=length,
        .noise_floor=-97,
        .channel=packet_channel,
        .timestamp=qemu_clock_get_ns(QEMU_CLOCK_REALTIME)/1000,
    };
    // These 4 bits are set if the mac addresses previously stored at 0x40 and 0x48
    // match the destination or bssid addresses in the frame
    if(match_mac_address(frame->destination_address,(uint8_t *)s->mem+0x40)) 
        pkt->damatch0=1;
    if(match_mac_address(frame->destination_address,(uint8_t *)s->mem+0x48)) 
        pkt->damatch1=1;
    if(match_mac_address(frame->bssid_address,(uint8_t *)s->mem+0x40)) 
        pkt->bssidmatch0=1;
    if(match_mac_address(frame->bssid_address,(uint8_t *)s->mem+0x48))
        pkt->bssidmatch1=1;
    pkt->damatch0 = 1;
    pkt->bssidmatch0 = 1;

    memcpy(header+sizeof(wifi_pkt_rx_ctrl_c3_t),frame,length);
    length+=sizeof(wifi_pkt_rx_ctrl_c3_t);
    // do a DMA transfer from the hardware to esp32 memory
    dma_list_item item;
    address_space_read(&address_space_memory, s->dma_inlink_address,
                       MEMTXATTRS_UNSPECIFIED, &item, sizeof(item));
    address_space_write(&address_space_memory, item.address,
                        MEMTXATTRS_UNSPECIFIED, header, length);
    item.length = length;
    item.eof = 1;
    address_space_write(&address_space_memory, s->dma_inlink_address,
                        MEMTXATTRS_UNSPECIFIED, &item, sizeof(uint32_t));
    s->mem[0x8c / sizeof(uint32_t)] = item.next;
    s->mem[0x90 / sizeof(uint32_t)] = s->dma_inlink_address;
    if (item.next) {
        s->dma_inlink_address = item.next;
    } else {
        uint32_t base = s->mem[A_C3_WIFI_DMA_INLINK / sizeof(uint32_t)];
        uint32_t offset = base & 0xfffff;
        s->dma_inlink_address = offset ? 0x3fc00000 | offset : 0;
    }
    set_interrupt(s, 0x1004024);  
}

static const MemoryRegionOps esp32C3_wifi_ops = {
    .read =  esp32C3_wifi_read,
    .write = esp32C3_wifi_write,
    .endianness = DEVICE_LITTLE_ENDIAN,
};

static void esp32c3_wifi_reset(DeviceState *dev)
{
    Esp32WifiState *s = ESP32_WIFI(dev);

    s->dma_inlink_address=0;
    memset(s->mem,0,sizeof(s->mem));
    Esp32_WLAN_reset_ap(s);
}

static void esp32C3_wifi_realize(DeviceState *dev, Error **errp)
{
    Esp32WifiState *s = ESP32_WIFI(dev);
    SysBusDevice *sbd = SYS_BUS_DEVICE(dev);
    s->dma_inlink_address=0;

    memory_region_init_io(&s->iomem, OBJECT(dev), &esp32C3_wifi_ops, s,
                          TYPE_ESP32_WIFI, 0x1000);
    sysbus_init_mmio(sbd, &s->iomem);
    sysbus_init_irq(sbd, &s->irq);
    memset(s->mem,0,sizeof(s->mem));
    Esp32_WLAN_setup_ap(dev, s);
    
}
static Property esp32C3_wifi_properties[] = {
    DEFINE_NIC_PROPERTIES(Esp32WifiState, conf),
    DEFINE_PROP_END_OF_LIST(),
};

static void esp32C3_wifi_class_init(ObjectClass *klass, void *data)
{
    DeviceClass *dc = DEVICE_CLASS(klass);

    dc->realize = esp32C3_wifi_realize;
    device_class_set_legacy_reset(dc, esp32c3_wifi_reset);
    set_bit(DEVICE_CATEGORY_NETWORK, dc->categories);
    dc->desc = "Esp32C3 WiFi";
    device_class_set_props(dc, esp32C3_wifi_properties);
}


static const TypeInfo esp32C3_wifi_info = {
    .name = TYPE_ESP32_WIFI,
    .parent = TYPE_SYS_BUS_DEVICE,
    .instance_size = sizeof(Esp32WifiState),
    .class_init    = esp32C3_wifi_class_init,
};

static void esp32C3_wifi_register_types(void)
{
    type_register_static(&esp32C3_wifi_info);
}

type_init(esp32C3_wifi_register_types)