- 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);
- return true;
+ 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);