wii ps3 bt

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <gccore.h>
#include <wiiuse/wpad.h>
#include <bte/bte.h>
#include <bte/bd_addr.h>
#include "l2cap.h"
#include "btpbuf.h"
#include "hci.h"

/* MAC1: 00:06:F7:4F:97:5C */
/* MAC2: 00:24:33:63:4B:6B */


int run = 1;
void *xfb = NULL;
err_t err;
GXRModeObj *rmode = NULL;
void init_video();
void print_mac(uint8_t *mac);
void WiiPowerPressed();
/*err_t l2cap_connect_ind(struct l2cap_pcb *npcb,
                         struct bd_addr *bdaddr,
                         u16_t psm,
                         err_t (* l2ca_connect_ind)(void *arg, struct l2cap_pcb *pcb, err_t err))
*/
err_t l2ca_connect_ind_ctrl(void *arg, struct l2cap_pcb *pcb, err_t err);
err_t l2ca_connect_ind_data(void *arg, struct l2cap_pcb *pcb, err_t err);
err_t l2ca_recv_ctrl(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err);
err_t l2ca_recv_data(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err);
err_t l2ca_disconnect_ind_ctrl(void *arg, struct l2cap_pcb *newpcb, err_t err);
err_t l2ca_disconnect_ind_data(void *arg, struct l2cap_pcb *newpcb, err_t err);
s32 read_local_addr_cb(s32 result,void *userdata);
struct l2cap_pcb *l2cap_ctrl, *l2cap_data;
struct bd_addr ps3_bdaddr = {.addr = {0x6B, 0x4B, 0x63, 0x33, 0x24, 0x00}};



int main(int argc, char **argv)
{
    SYS_SetPowerCallback(WiiPowerPressed);
    iosCreateHeap(4096);
    //l2cap_init();
    //l2cap_reset_all();
    //BTE_Init();
    WPAD_Init();
    init_video();
    usleep(100*1000);
    printf("ps3 controller to Wii:\n");


    /// Trying with l2cap_connect_ind

    if((l2cap_ctrl = l2cap_new()) == NULL) {
        printf("error creating l2cap_ctrl\n");
    } else {
        l2cap_arg(l2cap_ctrl, &ps3_bdaddr);
        err_t err = l2cap_connect_ind(l2cap_ctrl, &ps3_bdaddr, HIDP_CONTROL_CHANNEL, l2ca_connect_ind_ctrl);
        if (err == ERR_OK) {
            printf("connect l2cap_ctrl done\n");
        } else {
            l2cap_close(l2cap_ctrl);
            printf("error connect l2cap_ctrl");
        }
    }

    if((l2cap_data = l2cap_new()) == NULL) {
        printf("error creating l2cap_data\n");
    } else {
        l2cap_arg(l2cap_data, &ps3_bdaddr);
        //l2cap_recv(l2cap_data, l2ca_recv_data);
        //l2cap_disconnect_ind(l2cap_data, l2ca_disconnect_ind_data);
        err_t err = l2cap_connect_ind(l2cap_data, &ps3_bdaddr, HIDP_DATA_CHANNEL, l2ca_connect_ind_data);
        if (err == ERR_OK) {
            printf("connect l2cap_data done\n");
        } else {
            l2cap_close(l2cap_data);
            printf("error connect l2cap_data");
        }
    }

    printf("main\n");
    while(run) {
        WPAD_ScanPads();
        u32 pressed = WPAD_ButtonsDown(0);
        if (pressed & WPAD_BUTTON_A) {

        }
        if (pressed & WPAD_BUTTON_HOME) run = 0;
        usleep(10*1000);
        VIDEO_WaitVSync();
    }
    printf("\nexiting...");
    exit(0);
    return 0;
}

err_t l2ca_connect_ind_ctrl(void *arg, struct l2cap_pcb *pcb, err_t err)
{
    printf("CTRL incoming connection:\n");
    if (arg == NULL) {
        printf("NULL arg\n");
        return -3;
    }
    struct bd_addr *bdaddr = (struct bd_addr *)arg;
    unsigned char enable[] ATTRIBUTE_ALIGN(32) = {
        0x53, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE */
        0xf4, 0x42, 0x03, 0x00, 0x00 };

    unsigned char setleds[] ATTRIBUTE_ALIGN(32) = {
        0x52, /* HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT */
        0x01,
        0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x1E,
        0xff, 0x27, 0x10, 0x00, 0x32,
        0xff, 0x27, 0x10, 0x00, 0x32,
        0xff, 0x27, 0x10, 0x00, 0x32,
        0xff, 0x27, 0x10, 0x00, 0x32,
        0x00, 0x00, 0x00, 0x00, 0x00,
    };
    const unsigned char ledpattern[7] ATTRIBUTE_ALIGN(32) = {
        0x02, 0x04, 0x08, 0x10,
        0x12, 0x14, 0x18 };

    l2cap_recv(l2cap_ctrl, l2ca_recv_ctrl);
    l2cap_disconnect_ind(l2cap_ctrl, l2ca_disconnect_ind_ctrl);

    struct pbuf *p_enable = btpbuf_alloc(PBUF_RAW, sizeof(enable), PBUF_RAM);
    memcpy(p_enable->payload, enable, sizeof(enable));
    int i;
    for (i = 0; i< sizeof(enable); ++i) {
        printf(", 0x%X", ((unsigned char*)p_enable->payload)[i]);
    }
   // err_t e = l2ca_datawrite(pcb, p_enable);
    l2cap_write(bdaddr,p_enable,sizeof(enable));
    //printf("\ndatawrite returned: %d\n", e);
    btpbuf_free(p_enable);



    struct pbuf *p_setleds = btpbuf_alloc(PBUF_RAW, sizeof(setleds), PBUF_RAM);
    setleds[11] = ledpattern[1];
    memcpy(p_setleds->payload, setleds, sizeof(setleds));
    //l2ca_datawrite(pcb, p_setleds);
    l2cap_write(bdaddr, p_setleds,sizeof(setleds));

    btpbuf_free(p_setleds);
    printf("sent stuff\n");
/*
    // enable reporting
    send(client[i].ctrl, enable, sizeof(enable), 0);
    recv(client[i].ctrl, buf, sizeof(buf), 0);

    //set LEDs
    if (i < 7)
        setleds[11] = ledpattern[i];
    send(client[i].ctrl, setleds, sizeof(setleds), 0);
    recv(client[i].ctrl, buf, sizeof(buf), 0);
*/
    return 1;
}

err_t l2ca_connect_ind_data(void *arg, struct l2cap_pcb *pcb, err_t err)
{
    printf("DATA incoming connection:\n");

    return 1;
}


err_t l2ca_recv_ctrl(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err)
{
    printf("CTRL data received!:\n");

    return 1;
}

err_t l2ca_recv_data(void *arg, struct l2cap_pcb *pcb, struct pbuf *p, err_t err)
{
    printf("DATA data received!:\n");

    return 1;
}

err_t l2ca_disconnect_ind_ctrl(void *arg, struct l2cap_pcb *newpcb, err_t err)
{
    printf("DISCONNECT CTRL!\n");

    return 1;
}

err_t l2ca_disconnect_ind_data(void *arg, struct l2cap_pcb *newpcb, err_t err)
{
    printf("DISCONNECT DATA!\n");

    return 1;
}

void print_mac(uint8_t *mac)
{
    printf("%02X:%02X:%02X:%02X:%02X:%02X\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
}

void WiiPowerPressed()
{
    run = 0;
}

void init_video()
{
    VIDEO_Init();
    rmode = VIDEO_GetPreferredMode(NULL);
    xfb = MEM_K0_TO_K1(SYS_AllocateFramebuffer(rmode));
    console_init(xfb,20,20,rmode->fbWidth,rmode->xfbHeight,rmode->fbWidth*VI_DISPLAY_PIX_SZ);
    VIDEO_Configure(rmode);
    VIDEO_SetNextFramebuffer(xfb);
    VIDEO_SetBlack(FALSE);
    VIDEO_Flush();
    VIDEO_WaitVSync();
    if(rmode->viTVMode&VI_NON_INTERLACE) VIDEO_WaitVSync();
    printf("\x1b[2;0H");
}