BeRTOS
|
00001 00079 #include "pocketbus.h" 00080 00081 #include "cfg/cfg_pocketbus.h" 00082 00083 // Define logging setting (for cfg/log.h module). 00084 #define LOG_LEVEL POCKETBUS_LOG_LEVEL 00085 #define LOG_VERBOSITY POCKETBUS_LOG_FORMAT 00086 #include <cfg/log.h> 00087 #include <cfg/debug.h> 00088 #include <cfg/macros.h> 00089 00090 #include <io/kfile.h> 00091 00092 #include <cpu/byteorder.h> 00093 00094 #include <string.h> 00095 00099 void pocketbus_putchar(struct PocketBusCtx *ctx, uint8_t c) 00100 { 00101 /* Update checksum */ 00102 rotating_update1(c, &ctx->out_cks); 00103 00104 /* Escape characters with special meaning */ 00105 if (c == POCKETBUS_ESC || c == POCKETBUS_STX || c == POCKETBUS_ETX) 00106 kfile_putc(POCKETBUS_ESC, ctx->fd); 00107 00108 kfile_putc(c, ctx->fd); 00109 } 00110 00114 void pocketbus_begin(struct PocketBusCtx *ctx, pocketbus_addr_t addr) 00115 { 00116 PocketBusHdr hdr; 00117 00118 hdr.ver = POCKETBUS_VER; 00119 hdr.addr = cpu_to_be16(addr); 00120 rotating_init(&ctx->out_cks); 00121 00122 /* Send STX */ 00123 kfile_putc(POCKETBUS_STX, ctx->fd); 00124 00125 /* Send header */ 00126 pocketbus_write(ctx, &hdr, sizeof(hdr)); 00127 } 00128 00132 void pocketbus_write(struct PocketBusCtx *ctx, const void *_data, size_t len) 00133 { 00134 const uint8_t *data = (const uint8_t *)_data; 00135 00136 while (len--) 00137 pocketbus_putchar(ctx, *data++); 00138 } 00139 00143 void pocketbus_end(struct PocketBusCtx *ctx) 00144 { 00145 /* Send checksum */ 00146 rotating_t cks = cpu_to_be16(ctx->out_cks); 00147 pocketbus_write(ctx, &cks, sizeof(cks)); 00148 00149 /* Send ETX */ 00150 kfile_putc(POCKETBUS_ETX, ctx->fd); 00151 } 00152 00156 void pocketbus_send(struct PocketBusCtx *ctx, pocketbus_addr_t addr, const void *data, size_t len) 00157 { 00158 pocketbus_begin(ctx, addr); 00159 00160 /* Send data */ 00161 pocketbus_write(ctx, data, len); 00162 00163 pocketbus_end(ctx); 00164 } 00165 00166 00171 bool pocketbus_recv(struct PocketBusCtx *ctx, struct PocketMsg *msg) 00172 { 00173 int c; 00174 00175 /* Process incoming characters until buffer is not empty */ 00176 while ((c = kfile_getc(ctx->fd)) != EOF) 00177 { 00178 /* Look for STX char */ 00179 if (c == POCKETBUS_STX && !ctx->escape) 00180 { 00181 /* When an STX is found, inconditionally start a new packet */ 00182 if (ctx->sync) 00183 kprintf("pocketBus double sync!\n"); 00184 00185 ctx->sync = true; 00186 ctx->len = 0; 00187 rotating_init(&ctx->in_cks); 00188 continue; 00189 } 00190 00191 if (ctx->sync) 00192 { 00193 /* Handle escape mode */ 00194 if (c == POCKETBUS_ESC && !ctx->escape) 00195 { 00196 ctx->escape = true; 00197 continue; 00198 } 00199 00200 /* Handle message end */ 00201 if (c == POCKETBUS_ETX && !ctx->escape) 00202 { 00203 ctx->sync = false; 00204 00205 /* Check minimum size */ 00206 if (ctx->len < sizeof(PocketBusHdr) + sizeof(rotating_t)) 00207 { 00208 kprintf("pocketBus short pkt!\n"); 00209 continue; 00210 } 00211 00212 /* Remove checksum bytes from packet len */ 00213 ctx->len -= sizeof(rotating_t); 00214 00215 /* Compute checksum */ 00216 rotating_update(ctx->buf, ctx->len, &ctx->in_cks); 00217 uint8_t cks_h = *(ctx->buf + ctx->len); 00218 uint8_t cks_l = *(ctx->buf + ctx->len + 1); 00219 00220 rotating_t recv_cks = (cks_h << 8) | cks_l; 00221 00222 /* Checksum check */ 00223 if (recv_cks == ctx->in_cks) 00224 { 00225 PocketBusHdr *hdr = (PocketBusHdr *)ctx; 00226 00227 /* Check packet version */ 00228 if (hdr->ver == POCKETBUS_VER) 00229 { 00230 /* Packet received, set msg fields */ 00231 msg->payload = ctx->buf + sizeof(PocketBusHdr); 00232 msg->addr = be16_to_cpu(hdr->addr); 00233 msg->len = ctx->len - sizeof(PocketBusHdr); 00234 msg->ctx = ctx; 00235 return true; 00236 } 00237 else 00238 { 00239 kprintf("pocketBus version mismatch, here[%d], there[%d]\n", POCKETBUS_VER, hdr->ver); 00240 continue; 00241 } 00242 } 00243 else 00244 { 00245 kprintf("pocketBus cks error, here[%04X], there[%04X]\n", ctx->in_cks, recv_cks); 00246 continue; 00247 } 00248 00249 } 00250 00251 ctx->escape = false; 00252 00253 /* Check buffer overflow: simply ignore 00254 received data and go to unsynced state. */ 00255 if (ctx->len >= CONFIG_POCKETBUS_BUFLEN) 00256 { 00257 kprintf("pocketBus buffer overflow\n"); 00258 ctx->sync = false; 00259 continue; 00260 } 00261 00262 /* Put received data in the buffer */ 00263 ctx->buf[ctx->len] = c; 00264 ctx->len++; 00265 } 00266 } 00267 00268 /* 00269 * Check stream status. 00270 */ 00271 if (kfile_error(ctx->fd)) 00272 { 00273 LOG_ERR("fd status[%04X]\n", kfile_error(ctx->fd)); 00274 kfile_clearerr(ctx->fd); 00275 } 00276 00277 return false; 00278 } 00279 00280 00284 void pocketbus_init(struct PocketBusCtx *ctx, struct KFile *fd) 00285 { 00286 ASSERT(ctx); 00287 ASSERT(fd); 00288 00289 memset(ctx, 0, sizeof(*ctx)); 00290 ctx->fd = fd; 00291 }