#include <stdlib.h>
#include <string.h>
-#ifndef STM
-#include <unistd.h>
-#include <stdio.h>
-#endif
-
#include "interface.h"
-#include "interpret.h"
#include "sds.h"
+#include "mem.h"
-struct sds sdss[MAXSDSS];
-uint8_t c;
+extern uint8_t *mem_top, *mem_bottom, *mem_task, *mem_sds;
-void sds_init(void)
+struct sds *sds_head(void)
{
- memset(&sdss, 0, sizeof(struct sds)*MAXSDSS);
+ return mem_sds == mem_top ? NULL
+ : (struct sds *)(mem_top-sizeof(struct sds));
}
-void sds_register(void)
+struct sds *sds_next(struct sds *s)
{
- uint8_t cs;
- for(cs = 0; cs<MAXSDSS; cs++)
- if(!sdss[cs].used)
+ uint8_t *next = (uint8_t *)s - sizeof(struct sds);
+ return next < mem_sds ? NULL : (struct sds *)next;
+}
+
+struct sds *sds_get(uint8_t id)
+{
+ struct sds *s = sds_head();
+ while(s != NULL){
+ if(s->id == id){
break;
+ }
+ s = sds_next(s);
+ }
+ return s;
+}
- if(cs == MAXSDSS)
- die("Trying to add too much sdss...");
+void sds_register(void)
+{
+ debug("free memory: %lu", mem_free());
+
+ if(mem_sds-sizeof(struct sds) < mem_task){
+ die("Out of memory... Not enough to allocate sds struct");
+ }
+
+ struct sds *s = (struct sds *)(mem_sds-sizeof(struct sds));
- memset(&sdss[cs], 0, sizeof(struct sds));
//Read identifier
- sdss[cs].id = read16();
- //Read value
- sdss[cs].type = read_byte();
- sdss[cs].value = read16();
+ s->id = read16();
- debug("Received sds %d: %d", sdss[cs].id, sdss[cs].value);
- sdss[cs].used = true;
+ //Read value
+ s->type = read_byte();
+ s->value = read16();
write_byte('s');
- write16(sdss[cs].id);
+ write16(s->id);
write_byte('\n');
+
+ mem_sds -= sizeof(struct sds);
}
-void sds_delete(void)
+void sds_delete(uint8_t c)
{
- uint8_t cs;
- cs = read16();
- sdss[cs].used = false;
- write_byte('a');
- write16(sdss[cs].id);
- write_byte('\n');
+ struct sds *s = sds_get(c);
+
+ if(s != NULL){
+ //We found the sds, now we move verything from the end of the sds up to
+ //the pointer to the right
+ uint8_t *end = (uint8_t *)s;
+ uint8_t *start = end + sizeof(struct sds) - 1;
+ for(int i = 0; i<end-mem_sds; i++){
+ *(start - i) = *(end - i-1);
+ }
+ mem_sds += start-end+1;
+ }
}
-bool sds_update(void)
+bool sds_update(uint8_t id)
{
- uint8_t cs, id;
//Read identifier
- id = read16();
-
- for(cs = 0; cs<MAXSDSS; cs++){
- if(!sdss[cs].used)
- continue;
- if(sdss[cs].id == id){
- //Read value
- sdss[cs].value = read16();
- debug("Received sds update %d: %d",
- sdss[cs].id, sdss[cs].value);
- write_byte('u');
- write16(sdss[cs].id);
- write_byte('\n');
- return true;
+ struct sds *s = sds_get(id);
+ if(s != NULL){
+ switch(read_byte()){
+ //Long
+ case 'l':
+ //Int
+ case 'i':
+ s->value = read16();
+ break;
+ case 'b': //Bool
+ case 'c': //Character
+ case 'B': //Button
+ case 'L': //UserLED
+ s->value = read_byte();
+ break;
}
+ debug("Received sds update %d: %d", s->id, s->value);
+ write_byte('u');
+ write16(s->id);
+ write_byte('\n');
+ return true;
}
return false;
}
void sds_publish(int id)
{
- uint8_t cs;
- for(cs = 0; cs<MAXSDSS; cs++){
- if(sdss[cs].used && sdss[cs].id == id){
- debug("Publish %d=%d", sdss[cs].id, sdss[cs].value);
- write_byte('p');
- write16(sdss[cs].id);
- write_byte(sdss[cs].type);
-
- switch(sdss[cs].type){
- //Long
- case 'l':
- //Int
- case 'i':
- write16(sdss[cs].value);
- break;
- case 'b': //Bool
- case 'c': //Character
- case 'B': //Button
- case 'L': //UserLED
- write_byte(sdss[cs].value);
- break;
- }
- write_byte('\n');
- return;
+ struct sds *s = sds_get(id);
+ if(s != NULL){
+ debug("Publish %d=%d", s->id, s->value);
+ write_byte('p');
+ write16(s->id);
+ write_byte(s->type);
+
+ switch(s->type){
+ //Long
+ case 'l':
+ //Int
+ case 'i':
+ write16(s->value);
+ break;
+ case 'b': //Bool
+ case 'c': //Character
+ case 'B': //Button
+ case 'L': //UserLED
+ write_byte(s->value);
+ break;
}
+ write_byte('\n');
+ return;
+ } else {
+ debug("SDS identifier unknown: %d", id);
+ die("");
}
- debug("SDS identifier unknown: %d", id);
- die("");
}
int sds_fetch(int id)
{
- uint8_t cs;
- for(cs = 0; cs<MAXSDSS; cs++)
- if(sdss[cs].used && sdss[cs].id == id)
- return sdss[cs].value;
+ struct sds *s = sds_get(id);
+ if(s != NULL){
+ return s->value;
+ }
debug("SDS identifier unknown: %d", id);
die("");
return 0;
void sds_store(int id, int val)
{
- uint8_t cs;
- for(cs = 0; cs<MAXSDSS; cs++) {
- if(sdss[cs].used && sdss[cs].id == id){
- sdss[cs].value = val;
- return;
- }
+ struct sds *s = sds_get(id);
+ if(s != NULL){
+ s->value = val;
+ } else {
+ debug("SDS identifier unknown: %d", id);
+ die("");
}
- debug("SDS identifier unknown: %d", id);
- die("");
}