diff options
Diffstat (limited to 'converter/other/srf.c')
| -rw-r--r-- | converter/other/srf.c | 653 |
1 files changed, 653 insertions, 0 deletions
diff --git a/converter/other/srf.c b/converter/other/srf.c new file mode 100644 index 00000000..b0f97242 --- /dev/null +++ b/converter/other/srf.c @@ -0,0 +1,653 @@ +/* + * Funcs for working with SRF (Garmin vehicle) files + * + * Written by Mike Frysinger <vapier@gentoo.org> + * Released into the public domain + */ + +#include <stdio.h> + +#include "pm_c_util.h" +#include "mallocvar.h" +#include "nstring.h" +#include "srf.h" + + +static unsigned char +csumRaw(void * const p, + size_t const len) { + + unsigned char retval; + + unsigned int i; + unsigned char * c; + + for (i = 0, retval = 0, c = p; i < len; ++i) + retval += *c++; + + return retval; +} + + + +static unsigned char +csumPstring(struct srf_pstring * const pstringP) { + + return + csumRaw(&pstringP->len, 4) + csumRaw(pstringP->val, pstringP->len); +} + + + +static bool +readPstring(FILE * const ifP, + struct srf_pstring * const pstringP) { + + size_t bytesRead; + + pm_readlittlelong2u(ifP, &pstringP->len); + + MALLOCARRAY(pstringP->val, pstringP->len + 1); + + if (!pstringP->val) + pm_error("Failed to allocate buffer to read %u-byte pstring", + pstringP->len); + + bytesRead = fread(pstringP->val, 1, pstringP->len, ifP); + if (bytesRead != pstringP->len) + pm_error("Failed to read pstring. Requested %u bytes, got %u", + (unsigned)pstringP->len, (unsigned)bytesRead); + + pstringP->val[pstringP->len] = '\0'; + + return true; +} + + + +static bool +writePstring(FILE * const ofP, + struct srf_pstring * const pstringP) { + + bool retval; + size_t bytesWritten; + + pm_writelittlelongu(ofP, pstringP->len); + + bytesWritten = fwrite(pstringP->val, 1, pstringP->len, ofP); + + if (bytesWritten == pstringP->len) + retval = true; + else + retval = false; + + return retval; +} + + + +static size_t +lenHeader(struct srf_header * const headerP) { + + return 16 + (4 * 4) + 4 + headerP->s578.len + + 4 + 4 + headerP->ver.len + + 4 + 4 + headerP->prod.len; +} + + + +static unsigned char +csumHeader(struct srf_header * const headerP) { + + return + csumRaw(headerP->magic, 16) + + csumRaw(&headerP->_int4, 2 * 4) + + csumRaw(&headerP->img_cnt, 4) + + csumRaw(&headerP->_int5, 4) + + csumPstring(&headerP->s578) + + csumRaw(&headerP->_int6, 4) + + csumPstring(&headerP->ver) + + csumRaw(&headerP->_int7, 4) + + csumPstring(&headerP->prod); +} + + + +static bool +readHeader(FILE * const ifP, + struct srf_header * const headerP) { + + bool const retval = + fread(headerP->magic, 1, 16, ifP) == 16 && + pm_readlittlelong2u(ifP, &headerP->_int4[0]) == 0 && + pm_readlittlelong2u(ifP, &headerP->_int4[1]) == 0 && + pm_readlittlelong2u(ifP, &headerP->img_cnt) == 0 && + pm_readlittlelong2u(ifP, &headerP->_int5) == 0 && + readPstring(ifP, &headerP->s578) && + pm_readlittlelong2u(ifP, &headerP->_int6) == 0 && + readPstring(ifP, &headerP->ver) && + pm_readlittlelong2u(ifP, &headerP->_int7) == 0 && + readPstring(ifP, &headerP->prod); + + headerP->magic[16] = '\0'; + + return retval; +} + + + +static bool +writeHeader(FILE * const ofP, + struct srf_header * const headerP) { + + return + fwrite(headerP->magic, 1, 16, ofP) == 16 && + pm_writelittlelongu(ofP, headerP->_int4[0]) == 0 && + pm_writelittlelongu(ofP, headerP->_int4[1]) == 0 && + pm_writelittlelongu(ofP, headerP->img_cnt) == 0 && + pm_writelittlelongu(ofP, headerP->_int5) == 0 && + writePstring(ofP, &headerP->s578) && + pm_writelittlelongu(ofP, headerP->_int6) == 0 && + writePstring(ofP, &headerP->ver) && + pm_writelittlelongu(ofP, headerP->_int7) == 0 && + writePstring(ofP, &headerP->prod); +} + + + +static bool +checkHeader(struct srf_header * const headerP) { + + return + streq(headerP->magic, SRF_MAGIC) && + headerP->_int4[0] == 4 && + headerP->_int4[1] == 4 && + /* Should we require img_cnt to be multiple of 2 ? */ + headerP->img_cnt > 0 && + headerP->_int5 == 5 && + headerP->s578.len == 3 && + strcmp(headerP->s578.val, "578") == 0 && + headerP->_int6 == 6 && + headerP->ver.len == 4 && + /* Allow any headerP->ver value */ + headerP->_int7 == 7 && + headerP->prod.len == 12; + /* Allow any headerP->prod value */ +} + + + +static size_t +lenImg(struct srf_img * const imgP) { + + return + (4 * 3) + (2 * 2) + (1 * 2) + 2 + 4 + + 4 + 4 + imgP->alpha.data_len + + 4 + 4 + imgP->data.data_len; +} + + + +static unsigned char +csumImg(struct srf_img * const imgP) { + + struct srf_img_header * const headerP = &imgP->header; + struct srf_img_alpha * const alphaP = &imgP->alpha; + struct srf_img_data * const dataP = &imgP->data; + + return + csumRaw(&headerP->_ints, 4 * 3) + + csumRaw(&headerP->height, 2) + + csumRaw(&headerP->width, 2) + + csumRaw(headerP->_bytes, 2) + + csumRaw(&headerP->line_len, 2) + + csumRaw(&headerP->zeros, 4) + + csumRaw(&alphaP->type, 4) + + csumRaw(&alphaP->data_len, 4) + + csumRaw(alphaP->data, alphaP->data_len) + + csumRaw(&dataP->type, 4) + + csumRaw(&dataP->data_len, 4) + + csumRaw(dataP->data, dataP->data_len); +} + + + +static bool +readImgHeader(FILE * const ifP, + struct srf_img_header * const headerP) { + return + pm_readlittlelong2u(ifP, &headerP->_ints[0]) == 0 && + pm_readlittlelong2u(ifP, &headerP->_ints[1]) == 0 && + pm_readlittlelong2u(ifP, &headerP->_ints[2]) == 0 && + pm_readlittleshortu(ifP, &headerP->height) == 0 && + pm_readlittleshortu(ifP, &headerP->width) == 0 && + fread(&headerP->_bytes[0], 1, 1, ifP) == 1 && + fread(&headerP->_bytes[1], 1, 1, ifP) == 1 && + pm_readlittleshortu(ifP, &headerP->line_len) == 0 && + pm_readlittlelong2u(ifP, &headerP->zeros) == 0; +} + + + +static bool +writeImgHeader(FILE * const ofP, + struct srf_img_header * const headerP) { + return + pm_writelittlelongu(ofP, headerP->_ints[0]) == 0 && + pm_writelittlelongu(ofP, headerP->_ints[1]) == 0 && + pm_writelittlelongu(ofP, headerP->_ints[2]) == 0 && + pm_writelittleshortu(ofP, headerP->height) == 0 && + pm_writelittleshortu(ofP, headerP->width) == 0 && + fwrite(&headerP->_bytes[0], 1, 1, ofP) == 1 && + fwrite(&headerP->_bytes[1], 1, 1, ofP) == 1 && + pm_writelittleshortu(ofP, headerP->line_len) == 0 && + pm_writelittlelongu(ofP, headerP->zeros) == 0; +} + + + +static bool +checkImgHeader(struct srf_img_header * const headerP) { + + return + headerP->_ints[0] == 0 && + headerP->_ints[1] == 16 && + headerP->_ints[2] == 0 && + headerP->_bytes[0] == 16 && + headerP->_bytes[1] == 8 && + headerP->line_len == headerP->width * 2 && + headerP->zeros == 0; +} + + + +static bool +readImgAlpha(FILE * const ifP, + struct srf_img_alpha * const alphaP) { + + bool retval; + + pm_readlittlelong2u(ifP, &alphaP->type); + pm_readlittlelong2u(ifP, &alphaP->data_len); + + MALLOCARRAY(alphaP->data, alphaP->data_len); + + if (!alphaP->data) + retval = false; + else { + size_t bytesRead; + + bytesRead = fread(alphaP->data, 1, alphaP->data_len, ifP); + retval = (bytesRead ==alphaP->data_len); + } + return retval; +} + + + +static bool +writeImageAlpha(FILE * const ofP, + struct srf_img_alpha * const alphaP) { + + return + pm_writelittlelongu(ofP, alphaP->type) == 0 && + pm_writelittlelongu(ofP, alphaP->data_len) == 0 && + fwrite(alphaP->data, 1, alphaP->data_len, ofP) == alphaP->data_len; +} + + + +static bool +checkImgAlpha(struct srf_img_alpha * const alphaP) { + + return (alphaP->type == 11); +} + + + +static bool +readImgData(FILE * const ifP, + struct srf_img_data * const dataP) { + + bool retval; + + pm_readlittlelong2u(ifP, &dataP->type); + pm_readlittlelong2u(ifP, &dataP->data_len); + + MALLOCARRAY(dataP->data, dataP->data_len / 2); + + if (!dataP->data) + retval = false; + else { + size_t bytesRead; + bytesRead = fread(dataP->data, 2, dataP->data_len / 2, ifP); + + retval = (bytesRead == dataP->data_len / 2); + } + return retval; +} + + + +static bool +writeImgData(FILE * const ofP, + struct srf_img_data * const dataP) { + + return + pm_writelittlelongu(ofP, dataP->type) == 0 && + pm_writelittlelongu(ofP, dataP->data_len) == 0 && + fwrite(dataP->data, 2, dataP->data_len / 2, ofP) + == dataP->data_len / 2; +} + + + +static bool +checkImgData(struct srf_img_data * const dataP) { + return dataP->type == 1; +} + + + +static bool +readImg(FILE * const ifP, + bool const verbose, + uint32_t const i, + struct srf_img * const imgP) { + + if (!readImgHeader(ifP, &imgP->header)) + pm_error("short srf image %u header", i); + if (!checkImgHeader(&imgP->header)) + pm_error("invalid srf image %u header", i); + + if (verbose) + pm_message("reading srf 16-bit RGB %ux%u image %u", + imgP->header.width, imgP->header.height, i); + + if (!readImgAlpha(ifP, &imgP->alpha)) + pm_error("short srf image %u alpha mask", i); + if (!checkImgAlpha(&imgP->alpha)) + pm_error("invalid srf image %u alpha mask", i); + + if (!readImgData(ifP, &imgP->data)) + pm_error("short srf image %u data", i); + if (!checkImgData(&imgP->data)) + pm_error("invalid srf image %u data", i); + + return true; +} + + + +static bool +writeImg(FILE * const ofP, + uint32_t const i, + struct srf_img * const imgP) { + + if (!checkImgHeader(&imgP->header)) + pm_error("invalid srf image %u header", i); + if (!writeImgHeader(ofP, &imgP->header)) + pm_error("short srf image %u header", i); + + if (!checkImgAlpha(&imgP->alpha)) + pm_error("invalid srf image %u alpha mask", i); + if (!writeImageAlpha(ofP, &imgP->alpha)) + pm_error("short srf image %u alpha mask", i); + + if (!checkImgData(&imgP->data)) + pm_error("invalid srf image %u data", i); + if (!writeImgData(ofP, &imgP->data)) + pm_error("short srf image %u data", i); + + return true; +} + + + +static uint8_t +csum(struct srf * const srfP, + size_t const padLen) { +/*---------------------------------------------------------------------------- + The sum of everything in the SRF image except the checksum byte. The + checksum byte is supposed to be the arithmetic opposite of this so that the + sum of everything is zero. +-----------------------------------------------------------------------------*/ + unsigned char retval; + unsigned int i; + + retval = csumHeader(&srfP->header); + + for (i = 0; i < srfP->header.img_cnt; ++i) + retval += csumImg(&srfP->imgs[i]); + + for (i = 0; i < padLen; ++i) + retval += 0xff; + + return retval; +} + + + +void +srf_read(FILE * const ifP, + bool const verbose, + struct srf * const srfP) { + + uint8_t trialCsum; + size_t padLen; + unsigned char pad[256]; + unsigned int i; + + if (!readHeader(ifP, &srfP->header)) + pm_error("short srf header"); + if (!checkHeader(&srfP->header)) + pm_error("invalid srf header"); + + if (verbose) + pm_message("reading srf ver %s with prod code %s and %u images", + srfP->header.ver.val, srfP->header.prod.val, + srfP->header.img_cnt); + + MALLOCARRAY(srfP->imgs, srfP->header.img_cnt); + + if (!srfP->imgs) + pm_error("Could not allocate memory for %u images", + srfP->header.img_cnt); + + for (i = 0; i < srfP->header.img_cnt; ++i) + if (!readImg(ifP, verbose, i, &srfP->imgs[i])) + pm_error("invalid srf image %u", i); + + padLen = fread(pad, 1, sizeof(pad), ifP); + if (!feof(ifP)) { + pm_errormsg("excess data at end of file"); + return; + } + + trialCsum = csum(srfP, 0); /* initial value */ + for (i = 0; i < padLen; ++i) + trialCsum += pad[i]; + if (trialCsum != 0) + pm_errormsg("checksum does not match"); +} + + + +void +srf_write(FILE * const ofP, + struct srf * const srfP) { + + uint8_t srfCsum; /* checksum value in SRF image */ + size_t padLen; + unsigned int i; + size_t bytesWritten; + + padLen = 1; /* initial value */ + + if (!checkHeader(&srfP->header)) + pm_error("invalid srf header"); + if (!writeHeader(ofP, &srfP->header)) + pm_error("write srf header"); + padLen += lenHeader(&srfP->header); + + for (i = 0; i < srfP->header.img_cnt; ++i) { + if (!writeImg(ofP, i, &srfP->imgs[i])) + pm_error("invalid srf image %u", i); + padLen += lenImg(&srfP->imgs[i]); + } + + /* Pad to 256 bytes */ + padLen = 256 - (padLen % 256); + if (padLen) { + char * d; + size_t bytesWritten; + + MALLOCARRAY(d, padLen); + + if (!d) + pm_error("Could not allocate memory for %u bytes of padding", + (unsigned)padLen); + + memset(d, 0xff, padLen); + + bytesWritten = fwrite(d, 1, padLen, ofP); + + if (bytesWritten != padLen) + pm_error("unable to 0xff pad file"); + + free(d); + } + + /* Write out checksum byte */ + srfCsum = 0xff - csum(srfP, padLen) + 1; + bytesWritten = fwrite(&srfCsum, 1, 1, ofP); + if (bytesWritten != 1) + pm_error("unable to write checksum"); +} + + + +static void +freeImg(struct srf_img * const imgP) { + + free(imgP->alpha.data); + free(imgP->data.data); +} + + + +void +srf_term(struct srf * const srfP) { + + unsigned int i; + + free(srfP->header.s578.val); + free(srfP->header.ver.val); + free(srfP->header.prod.val); + + for (i = 0; i < srfP->header.img_cnt; ++i) + freeImg(&srfP->imgs[i]); + + free(srfP->imgs); +} + + + +static void +srf_img_init(struct srf_img * const imgP, + uint16_t const width, + uint16_t const height) { + + struct srf_img_header * const headerP = &imgP->header; + struct srf_img_alpha * const alphaP = &imgP->alpha; + struct srf_img_data * const dataP = &imgP->data; + + headerP->_ints[0] = 0; + headerP->_ints[1] = 16; + headerP->_ints[2] = 0; + headerP->height = height; + headerP->width = width; + headerP->_bytes[0] = 16; + headerP->_bytes[1] = 8; + headerP->line_len = width * 2; + headerP->zeros = 0; + + alphaP->type = 11; + alphaP->data_len = height * width; + MALLOCARRAY(alphaP->data, alphaP->data_len); + + if (!alphaP->data) + pm_error("Could not allocate buffer for %u bytes of alpha", + alphaP->data_len); + + dataP->type = 1; + dataP->data_len = height * width * 2; + MALLOCARRAY(dataP->data, dataP->data_len / 2); + + if (!dataP->data) + pm_error("Could not allocation buffer for %u units of data", + dataP->data_len); +} + + + +static void +initPstring(struct srf_pstring * const pstringP, + const char * const s) { + + pstringP->len = strlen(s); + + MALLOCARRAY(pstringP->val, pstringP->len + 1); + + if (!pstringP->val) + pm_error("Could not allocate memory for string of length %u", + pstringP->len); + + memcpy(pstringP->val, s, pstringP->len + 1); +} + + + +void +srf_init(struct srf * const srfP) { + + struct srf_header * const headerP = &srfP->header; + + strcpy(headerP->magic, SRF_MAGIC); + headerP->_int4[0] = 4; + headerP->_int4[1] = 4; + headerP->img_cnt = 0; + headerP->_int5 = 5; + initPstring(&headerP->s578, "578"); + headerP->_int6 = 6; + initPstring(&headerP->ver, "1.00"); + headerP->_int7 = 7; + initPstring(&headerP->prod, "006-D0578-XX"); + + srfP->imgs = NULL; +} + + + +void +srf_create_img(struct srf * const srfP, + uint16_t const width, + uint16_t const height) { +/*---------------------------------------------------------------------------- + Add an "image" to the SRF. An image is a horizontal series of 36 + square frames, each showing a different angle view of an object, 10 + degrees about. At least that's what it's supposed to be. We don't + really care -- it's just an arbitrary rectangular raster image to us. +-----------------------------------------------------------------------------*/ + + ++srfP->header.img_cnt; + + REALLOCARRAY(srfP->imgs, srfP->header.img_cnt); + + if (!srfP->imgs) + pm_error("Could not allocate memory for %u images", + srfP->header.img_cnt); + + srf_img_init(&srfP->imgs[srfP->header.img_cnt-1], width, height); +} + |