#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <serial.h>
#include <uucode.h>
#include <lpc.h>

const struct lpc_part lpc_parts[] = {
  {"LPC1111/101", 0x041e502b, 2, 8},
  {"LPC1111/101 or 102", 0x2516d02b, 2, 8},
  {"LPC1111/201", 0x0416502b, 4, 8},
  {"LPC1111/201 or 202", 0x2516902b, 4, 8},
  
  {"LPC1112/101", 0x042d502b, 2, 16},
  {"LPC1112/101 or 102", 0x2524d02b, 2, 16},
  {"LPC1112/201", 0x0425502b, 4, 16},
  {"LPC1112/201 or 202", 0x2524902b, 4, 16},
  
  {"LPC1113/201", 0x0434502b, 4, 24},
  {"LPC1113/201 or 202", 0x2532902b, 4, 24},
  {"LPC1113/301", 0x0434102b, 8, 24},
  {"LPC1113/301 or 302", 0x2532102b, 8, 24},
  
  {"LPC1114/201", 0x0444502b, 4, 32},
  {"LPC1114/201 or 202", 0x2540902b, 4, 32},
  {"LPC1114/301", 0x0444102b, 8, 32},
  {"LPC1114/301 or 302", 0x2540102b, 8, 32},
  
  {"LPC11C12/301", 0x1421102b, 8, 16},
  {"LPC11C14/301", 0x1440102b, 8, 32},
  {"LPC11C22/301", 0x1431102b, 8, 16},
  {"LPC11C24/301", 0x1430102b, 8, 32},
  {NULL, 0, 0, 0}
};

const struct sprog_family lpc_family = {
  .setup = (void*(*)(struct serial_device*)) lpc_setup,
  .init = (void(*)(void*)) lpc_init,
  .exec = (void(*)(void*, const struct sprog_data*)) lpc_exec,
  .write = (void(*)(void*, const struct sprog_data*)) lpc_write,
  .close = (void(*)(void*)) lpc_close
};
  
  

void lpc_ispmode(struct lpc_device *dev, int state);
void lpc_reset(struct lpc_device *dev, int isp);
int lpc_scanf(struct lpc_device *dev, const char *text, ...);
void lpc_vprintf(struct lpc_device *dev, const char *text, va_list l);
void lpc_printf(struct lpc_device *dev, const char *text, ...);
int lpc_getline(struct lpc_device *dev, char *buf);
int lpc_command(struct lpc_device *dev, const char *text, ...);
int lpc_await_reply(struct lpc_device *dev, ...);
int lpc_read_partid(struct lpc_device *dev);
void lpc_write_ram(struct lpc_device *dev, const struct sprog_data *d, unsigned int addr);


struct lpc_device *lpc_setup(struct serial_device *port) {
  struct lpc_device *dev;
  dev = malloc(sizeof(struct lpc_device));
  dev->port = port;
  dev->part = NULL;
  dev->reply_time = 5000;
  lpc_reset(dev, 1);
  return dev;
}

void lpc_init(struct lpc_device *dev) {
  char buf[4096];

  serial_read(dev->port, buf, sizeof(buf), 0);
  
  serial_write(dev->port, "?");
  
  if(lpc_await_reply(dev, "Synchronized", NULL)==1) {
      sprog_info("Synchronization successful\n");
    lpc_printf(dev, "Synchronized\r\n");
    if(lpc_await_reply(dev, "OK", NULL)!=1)
      sprog_error("Expected OK, received '%s'\n", buf);
    lpc_printf(dev, "12000\r\n");
    if(lpc_await_reply(dev, "OK", NULL)!=1)
      sprog_error("Expected OK, received '%s'\n", buf);
  } else {
    serial_write(dev->port, "\r\n");
    if(lpc_await_reply(dev, "?", NULL)==1)
      sprog_info("The device appears to be already synchronized\n");
    else
      sprog_error("Invalid device response\n");
    lpc_getline(dev, buf); /* receive invalid command reply */
  }
  if(lpc_read_partid(dev)==0)
    sprog_info("Found device: %s, %dkB Flash, %dkB RAM\n", dev->part->name, dev->part->flash, dev->part->ram);
}

void lpc_write(struct lpc_device *dev, const struct sprog_data *d) {
  static const int chunk_sizes[] = {256, 512, 1024, 4096, 0}; /* keep it sorted! */
  struct sprog_data chunk;
  int sect_start;
  int sect_end;
  int i;
  int j;
  int status;
  int offset;
  int chunk_size;
  int max_chunk_index;
  int data_size;
  offset = 0;
  
  chunk.data = NULL;
  
  lpc_command(dev, "U 23130\r\n"); /* unlock the device */
  
  sect_end = dev->part->flash/4 - 1; /* last sector = Flash size / 4kB - 1 */
  lpc_command(dev, "P 0 %d\r\n", sect_end);
  
  sprog_info("Erasing Flash memory... ");
  j = dev->reply_time;
  dev->reply_time = 5000;
  if(lpc_command(dev, "E 0 %d\r\n", sect_end))
    sprog_info("Error\n");
  else
    sprog_info("OK\n");
  dev->reply_time = j;
  
  while(d->size-offset>0) {
    chunk_size = d->size - offset;
    for(i=0; chunk_sizes[i]; i++) {
      if(chunk_sizes[i]>=chunk_size)
	break;
      if(chunk_sizes[i]+1024 > dev->part->ram * 1024) {
	i--;
	break;
      }
    }
    
    if(chunk_sizes[i]==0)
      i--;
    
    chunk_size = chunk_sizes[i];
    max_chunk_index = i;
    
    data_size = chunk_size;
    
    if(data_size > d->size-offset) {
      data_size = d->size - offset;
      /* we assume that all of the available chunk sizes are divisible by the first one */
      /* TODO: Optimize the following code, assuming that the chunk sizes are powers of 2 */
      chunk_size = data_size - data_size % chunk_sizes[0];
      if(data_size % chunk_sizes[0])
	chunk_size += chunk_sizes[0];
    }
    
      
    sprog_alloc_data(&chunk, chunk_size);
    
    for(i=0; i<data_size; i++)
      chunk.data[i] = d->data[offset+i];
    
    for(i=data_size; i<chunk_size; i++)
      chunk.data[i] = 0xff;
    
    chunk.size = chunk_size;
    
    if(offset==0) {
      /* calculate the checksum and put it at 0x1c in the vector table */
      j = 0;
      for(i=0; i<0x1c; i+=4)
	j += chunk.data[i] | chunk.data[i+1]<<8 | chunk.data[i+2]<<16 | chunk.data[i+3]<<24;
      j = -j;
      for(i=0; i<4; i++) {
	chunk.data[0x1c+i] = j & 0xff;
	j >>= 8;
      }
    }
    
    lpc_write_ram(dev, &chunk, 0x10000400);
    
    sprog_info("Copying to Flash...\n");
    
    j = max_chunk_index;
    status = 1;
    for(i=chunk_size; i>0; i-=chunk_sizes[j]) {
      for(j=j; j>=0; j--)
	if(chunk_sizes[j]<=i)
	  break;
	
      sect_start = offset/4096;
      sect_end = (offset+chunk_sizes[j])/4096;
      
      lpc_command(dev, "P %d %d\r\n", sect_start, sect_end); /* prepare sectors for write */
      if(lpc_command(dev, "C %d %u %d\r\n", offset+(chunk_size-i), 0x10000400+(chunk_size-i), chunk_sizes[j]))
	status = 0;
    }
    
    if(status)
      sprog_info("Done!\n");
    else
      sprog_info("Error\n");
    
    offset += data_size;
  }
}

void lpc_exec(struct lpc_device *dev, const struct sprog_data *d) {
  lpc_write_ram(dev, d, 0x10000400);
  lpc_command(dev, "U 23130\r\n");
  sprog_info("Executing code... ");
  if(lpc_command(dev, "G %u T\r\n", 0x10000400))
    sprog_info("Error\n");
  else
    sprog_info("OK\n");
}

void lpc_write_ram(struct lpc_device *dev, const struct sprog_data *d, unsigned int addr) {
  struct sprog_data tmp;
  int last_offset;
  int last_i;
  int offset;
  int checksum;
  char buf[64];
  int i;
  int j;
  
  sprog_info("Writing %d bytes\n", d->size);
  /* align data size to 4 */
  if(d->size & 3) {
    tmp.data = NULL;
    sprog_alloc_data(&tmp, d->size + 4);
    for(i=0; i<d->size; i++)
      tmp.data[i] = d->data[i];
    for(i=d->size; i & 3; i++)
      tmp.data[i] = 0;
    tmp.size = i;
    d = &tmp;
  }
  
  lpc_command(dev, "W %u %u\r\n", addr, d->size);
  
  last_i = 0;
  last_offset = 0;
  offset = 0;
  sprog_progress(offset, d->size);
  checksum = 0;
  
  for(i=0; (j = uuencode_line(d, buf, &offset, &checksum)); i++) {
    lpc_printf(dev, "%s\r\n", buf);
    if(j==2 || (i % 20)==19) {
      lpc_printf(dev, "%u\r\n", checksum);
      checksum = 0;
      
      switch(lpc_await_reply(dev, "OK", "RESEND", NULL)) {
	case 1:
	  last_offset = offset;
	  last_i = i;
	  break;
	case 2:
	default:
	  offset = last_offset;
	  i = last_i;
      }
    }
    sprog_progress(offset, d->size);
  }
  sprog_info("\n");
}

int lpc_await_reply(struct lpc_device *dev, ...) {
  char buf[4096];
  int i;
  const char *t;
  va_list l;
  va_start(l, dev);
  if(!lpc_getline(dev, buf))
    return 0;
  for(i=0; buf[i]; i++) {
    if(buf[i]=='\n') {
      buf[i] = 0;
      if(i>0) {
	if(buf[i-1]=='\r')
	  buf[i-1] = 0;
      }
      break;
    }
  }
  
  for(i=0; (t = va_arg(l, const char*)); i++) {
    if(strcmp(buf, t)==0)
      return i+1;
  }
  return -1;
}
  

int lpc_getline(struct lpc_device *dev, char *buf) {
  if(sprog_waitdata(dev->port, dev->reply_time)==0)
    return 0;
//  sprog_info("fgets...\n");
  fgets(buf, 4096, dev->port->f);
//  sprog_info("...fgets\n");
//  sprog_info("--> %s\n", buf);
  return 1;
}

int lpc_scanf(struct lpc_device *dev, const char *text, ...) {
  int r;
  char buf[4096];
  va_list l;
  va_start(l, text);
  if(!lpc_getline(dev, buf))
    return 0;
  r = vsscanf(buf, text, l);
  va_end(l);
  return r;
}

void lpc_vprintf(struct lpc_device *dev, const char *text, va_list l) {
  char buf[4096];
  vsprintf(buf, text, l);
  serial_write(dev->port, buf);
  lpc_getline(dev, buf);
}

void lpc_printf(struct lpc_device *dev, const char *text, ...) {
  va_list l;
  va_start(l, text);
  lpc_vprintf(dev, text, l);
  va_end(l);
}

int lpc_command(struct lpc_device *dev, const char *text, ...) {
  int res;
  va_list l;
  va_start(l, text);
  lpc_vprintf(dev, text, l);
  va_end(l);
  if(lpc_scanf(dev, "%d", &res)<1)
    return -1;
  return res;
}

int lpc_read_partid(struct lpc_device *dev) {
  int i;
  int res;
  unsigned int partid;
  res = lpc_command(dev, "J\r\n");
  if(res!=0)
    return res;
  lpc_scanf(dev, "%u", &partid);
  for(i=0; lpc_parts[i].name; i++)
    if(lpc_parts[i].part_id==partid)
      break;
  if(lpc_parts[i].name)
    dev->part = &lpc_parts[i];
  else
    return -1;
  return 0;
}

void lpc_ispmode(struct lpc_device *dev, int state) {
  serial_setline(dev->port, SERIAL_RTS, state);
}

void lpc_reset(struct lpc_device *dev, int isp) {
  /* TODO: configure the control lines */
  serial_setline(dev->port, SERIAL_DTR, 1);
  if(isp)
    lpc_ispmode(dev, 1);
  sprog_sleep(50);
  serial_setline(dev->port, SERIAL_DTR, 0);
  if(isp) {
    sprog_sleep(50);
  }
}

void lpc_close(struct lpc_device *dev) {
  lpc_ispmode(dev, 0);
  lpc_reset(dev, 0);
  serial_close(dev->port);
  free(dev);
}