#include <avr/interrupt.h>
 #include <stdio.h>
 #include "i2c.h"
+#include "bmc.h"
 #include "ipmb.h"
 
 #define TWCR_ACK TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(1<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(0<<TWWC);  
 #define TWCR_NACK TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(0<<TWEA)|(0<<TWSTA)|(0<<TWSTO)|(0<<TWWC);
 #define TWCR_RESET TWCR = (1<<TWEN)|(1<<TWIE)|(1<<TWINT)|(1<<TWEA)|(0<<TWSTA)|(1<<TWSTO)|(0<<TWWC);  
 
-static volatile unsigned char databuf[12];
-static volatile uint8_t pos = 0x00;
+volatile unsigned char i2c_databuf[12];
+volatile uint8_t i2c_len = 0x00;
+static volatile uint8_t i2c_pos = 0x00;
+volatile uint8_t i2c_done = 0x00;
 
 void i2c_init()
 {
+       TWBR = 0xff;
        TWAR = BMC_ADDR & 0xfe;
        TWDR = 0x00;
        TWCR &= ~((1<<TWSTA) | (1<<TWSTO));
        TWCR |= ((1<<TWEA) | (1<<TWEN) | (1<<TWIE)); 
-       printf("Status: 0x%02x\n", TW_STATUS);
        PORTC = 0x03;
 }
 
        }
 
        TWCR = ((1<<TWINT) | (1<<TWEN) | (1<<TWSTO));
+       while(!(TWCR & (1<<TWSTO))) {}
 
-       printf("I2C Data sent 0x%02x\n", TW_STATUS);
-
+#if 1
+       /* Timing problem... */
+       printf("I2C Data sent\n");
+#endif
 
 out:   
        TWDR = 0x00;
 
 ISR (TWI_vect, ISR_BLOCK)
 {
+       if (i2c_done)
+               TWCR_RESET;
+
        switch (TW_STATUS) {
                case TW_SR_SLA_ACK:
 #ifdef DEBUG
                        printf("I2C: Slave 0x%02x adressed\n", TWDR);
 #endif
-                       pos = 0x00;
-                       databuf[pos] = TWDR;
-                       pos++;
+                       i2c_pos = 0x00;
+                       i2c_databuf[i2c_pos] = TWDR;
+                       i2c_pos++;
                        TWCR_ACK;
                        break;
 
 #ifdef DEBUG
                        printf("I2C: Data received: 0x%02x\n", TWDR);
 #endif
-                       databuf[pos] = TWDR;
-                       pos++;
+                       i2c_databuf[i2c_pos] = TWDR;
+                       i2c_pos++;
                        TWCR_ACK;
                        break;
 
 #ifdef DEBUG
                        printf("I2C: STOP received\n");
 #endif
-                       decode_ipmb_pkt((unsigned char*)databuf, pos);
-                       pos = 0x00;
+                       i2c_len = i2c_pos;
+                       i2c_pos = 0x00;
+                       i2c_done = 0x01;
                        TWCR_RESET;
                        break;
 
 
+#include <string.h>
 #include <stdio.h>
 
+#include "i2c.h"
+#include "chassis.h"
 #include "ipmb.h"
 
 uint8_t ipmb_csum(unsigned char *buf, int len)
        return -csum;
 }
 
-void decode_ipmb_pkt(unsigned char *buf, int len)
+void ipmb_send(struct ipmb_resp *resp)
 {
+       unsigned char buf[24];
+       int len;
+#ifdef DEBUG
        int i;
-       struct ipmb_req req;
+#endif
 
-       req.rsSA = buf[0];
-       req.netFn = (buf[1]>>2)&0x3f;
-       req.rsLUN = (buf[1] & 0x03);
-       req.rqSA = buf[3];
-       req.rqSEQ = (buf[4]>>2)&0x3f;
-       req.rqLUN = (buf[4] & 0x03);
-       req.cmd = buf[5];
-       req.data = buf+6;
-       req.datalen = len - 6 - 1;
+       buf[0] = resp->rqSA;
+       buf[1] = ((resp->netFn)<<2) | (resp->rqLUN & 0x3);
+       buf[2] = ipmb_csum(buf, 2);
+       buf[3] = resp->rsSA;
+       buf[4] = ((resp->rqSEQ)<<2) | (resp->rsLUN & 0x3);
+       buf[5] = resp->cmd;
+       memcpy(buf+6, resp->data, resp->datalen);
+       len = resp->datalen + 7;
+       buf[len-1] = ipmb_csum(buf+3, len - 4);
+
+#ifdef DEBUG
+       printf("Sending: ");
+       for(i = 0; i < len; i++) {
+               printf("0x%02x ", buf[i]);
+       }
+       printf("\n");
+#endif
+
+       i2c_send(buf, len);
+
+}
+
+#ifdef DEBUG
+void ipmb_dump_req(struct ipmb_req *req)
+{
+       int i;
 
        printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n");
        printf("Connection Header:\n");
-       printf("\trs Slave Addr.: 0x%02x\n", req.rsSA);
-       printf("\tnetFn: 0x%02x, LUN: 0x%02x\n", req.netFn, req.rsLUN);
-       printf("\tChecksum: 0x%02x (%s)\n", buf[2],
-               (buf[2] == ipmb_csum(buf, 2)) ? "OK" : "Wrong");
+       printf("\trs Slave Addr.: 0x%02x\n", req->rsSA);
+       printf("\tnetFn: 0x%02x, LUN: 0x%02x\n", req->netFn, req->rsLUN);
        printf("Data:\n");
-       printf("\trq Slave Addr.: 0x%02x\n", req.rqSA);
-       printf("\trqSeq: 0x%02x, rqLUN: 0x%02x\n", req.rqSEQ, req.rqLUN);
-       printf("\tcmd: 0x%02x\n", req.cmd);
+       printf("\trq Slave Addr.: 0x%02x\n", req->rqSA);
+       printf("\trqSeq: 0x%02x, rqLUN: 0x%02x\n", req->rqSEQ, req->rqLUN);
+       printf("\tcmd: 0x%02x\n", req->cmd);
        printf("\tData: ");
-       for(i = 0; i < req.datalen; i++) {
-               printf("0x%02x ", req.data[i]);
+       for(i = 0; i < req->datalen; i++) {
+               printf("0x%02x ", req->data[i]);
        }
        printf("\n");
-       printf("\tChecksum: 0x%02x (%s)\n", buf[len-1],
-               (buf[len-1] == ipmb_csum(buf+3, len-4)) ? "OK" : "Wrong");
        printf("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
+}
+#endif
+
+void ipmb_invalid(struct ipmb_resp *resp)
+{
+#if 0
+       static unsigned char cmd_invalid[] = {0xc2};
+
+       resp->data = cmd_invalid;
+       resp->datalen = sizeof(cmd_invalid);
+       ipmb_send(resp);
+#endif
+}
+
+void ipmb_cmd(struct ipmb_req *req)
+{
+       struct ipmb_resp resp;
+       static unsigned char get_devid[] =
+               {IPMB_CC_NORMALLY, 0x42, 0x42, 0x01, 0x01, 0x51, 0xff /* Add. Dev. Supp */, 0x00, 0x00, 0x00, 0x00, 0x00 };
+       static unsigned char get_wd_timer[] =
+               {IPMB_CC_NORMALLY, 0x42, 0x00, 0x00, 0x04, 0xff, 0xff, 0xfe, 0xfe};
+       static unsigned char cc_normal[] =
+               {IPMB_CC_NORMALLY};
+
+       resp.rqSA = req->rqSA;
+       resp.netFn = req->netFn+1;
+       resp.rqLUN = req->rqLUN;
+       resp.rsSA = req->rsSA;
+       resp.rqSEQ = req->rqSEQ;
+       resp.rsLUN = req->rsLUN;
+       resp.cmd = req->cmd;
+
+       switch (req->netFn) {
+               case IPMB_NETFN_CHASSIS:
+                       switch (req->cmd) {
+                               case IPMB_CHASSIS_CONTROL:
+                                       chassis_control(*(req->data));
+                                       resp.data = cc_normal;
+                                       resp.datalen = sizeof(cc_normal);
+                                       ipmb_send(&resp);
+                                       break;
+                               default:
+#ifdef DEBUG
+                                       printf("Unknown chassis cmd 0x%02x\n", req->cmd);
+                                       ipmb_dump_req(req);
+#endif
+                                       ipmb_invalid(&resp);
+                                       break;
+                       }
+                       break;
+
+               case IPMB_NETFN_SENSOR_EVENT:
+                       switch (req->cmd) {
+                               default:
+#ifdef DEBUG
+                                       printf("Unknown sensor cmd 0x%02x\n", req->cmd);
+                                       ipmb_dump_req(req);
+#endif
+                                       ipmb_invalid(&resp);
+                                       break;
+                       }
+                       break;
+
+               case IPMB_NETFN_APP:
+                       switch (req->cmd) {
+                               case IPMB_APP_GET_DEVICE_ID:
+                                       resp.data = get_devid;
+                                       resp.datalen = sizeof(get_devid);
+                                       ipmb_send(&resp);
+                                       break;
+
+                               case IPMB_APP_GET_WATCHDOG_TIMER:
+                                       resp.data = get_wd_timer;
+                                       resp.datalen = sizeof(get_wd_timer);
+                                       ipmb_send(&resp);
+                                       break;
+
+                               default:
+#ifdef DEBUG
+                                       printf("Unknown app cmd 0x%02x\n", req->cmd);
+                                       ipmb_dump_req(req);
+#endif
+                                       ipmb_invalid(&resp);
+                                       break;
+                       }
+                       break;
+
+               case IPMB_NETFN_STORAGE:
+                       switch (req->cmd) {
+                               default:
+#ifdef DEBUG
+                                       printf("Unknown storage cmd 0x%02x\n", req->cmd);
+                                       ipmb_dump_req(req);
+#endif
+                                       ipmb_invalid(&resp);
+                                       break;
+                       }
+                       break;
+
+               default:
+#ifdef DEBUG
+                       printf("Unknown netFn 0x%02x\n", req->netFn);
+                       ipmb_dump_req(req);
+#endif
+                       ipmb_invalid(&resp);
+                       break;
+       }
+}
+
+void decode_ipmb_pkt(unsigned char *buf, int len)
+{
+       struct ipmb_req req;
 
        if ((buf[2] != ipmb_csum(buf, 2)) ||
-           (buf[len-1] == ipmb_csum(buf+3, len-4)))
+           (buf[len-1] != ipmb_csum(buf+3, len-4)))
                return; /* Checksum wrong */
 
-       //i2c_send((unsigned char*)"\x28\x00",1);
+       req.rsSA = buf[0];
+       req.netFn = (buf[1]>>2)&0x3f;
+       req.rsLUN = (buf[1] & 0x03);
+       req.rqSA = buf[3];
+       req.rqSEQ = (buf[4]>>2)&0x3f;
+       req.rqLUN = (buf[4] & 0x03);
+       req.cmd = buf[5];
+       req.data = buf+6;
+       req.datalen = len - 6 - 1;
+
+       ipmb_cmd(&req);
 }