mxsimage.c 50 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336
  1. /*
  2. * Freescale i.MX23/i.MX28 SB image generator
  3. *
  4. * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
  5. *
  6. * SPDX-License-Identifier: GPL-2.0+
  7. */
  8. #ifdef CONFIG_MXS
  9. #include <errno.h>
  10. #include <fcntl.h>
  11. #include <stdio.h>
  12. #include <string.h>
  13. #include <unistd.h>
  14. #include <limits.h>
  15. #include <openssl/evp.h>
  16. #include "imagetool.h"
  17. #include "mxsimage.h"
  18. #include "pbl_crc32.h"
  19. #include <image.h>
  20. /*
  21. * DCD block
  22. * |-Write to address command block
  23. * | 0xf00 == 0xf33d
  24. * | 0xba2 == 0xb33f
  25. * |-ORR address with mask command block
  26. * | 0xf00 |= 0x1337
  27. * |-Write to address command block
  28. * | 0xba2 == 0xd00d
  29. * :
  30. */
  31. #define SB_HAB_DCD_WRITE 0xccUL
  32. #define SB_HAB_DCD_CHECK 0xcfUL
  33. #define SB_HAB_DCD_NOOP 0xc0UL
  34. #define SB_HAB_DCD_MASK_BIT (1 << 3)
  35. #define SB_HAB_DCD_SET_BIT (1 << 4)
  36. /* Addr.n = Value.n */
  37. #define SB_DCD_WRITE \
  38. (SB_HAB_DCD_WRITE << 24)
  39. /* Addr.n &= ~Value.n */
  40. #define SB_DCD_ANDC \
  41. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
  42. /* Addr.n |= Value.n */
  43. #define SB_DCD_ORR \
  44. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  45. /* (Addr.n & Value.n) == 0 */
  46. #define SB_DCD_CHK_EQZ \
  47. (SB_HAB_DCD_CHECK << 24)
  48. /* (Addr.n & Value.n) == Value.n */
  49. #define SB_DCD_CHK_EQ \
  50. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
  51. /* (Addr.n & Value.n) != Value.n */
  52. #define SB_DCD_CHK_NEQ \
  53. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
  54. /* (Addr.n & Value.n) != 0 */
  55. #define SB_DCD_CHK_NEZ \
  56. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  57. /* NOP */
  58. #define SB_DCD_NOOP \
  59. (SB_HAB_DCD_NOOP << 24)
  60. struct sb_dcd_ctx {
  61. struct sb_dcd_ctx *dcd;
  62. uint32_t id;
  63. /* The DCD block. */
  64. uint32_t *payload;
  65. /* Size of the whole DCD block. */
  66. uint32_t size;
  67. /* Pointer to previous DCD command block. */
  68. uint32_t *prev_dcd_head;
  69. };
  70. /*
  71. * IMAGE
  72. * |-SECTION
  73. * | |-CMD
  74. * | |-CMD
  75. * | `-CMD
  76. * |-SECTION
  77. * | |-CMD
  78. * : :
  79. */
  80. struct sb_cmd_list {
  81. char *cmd;
  82. size_t len;
  83. unsigned int lineno;
  84. };
  85. struct sb_cmd_ctx {
  86. uint32_t size;
  87. struct sb_cmd_ctx *cmd;
  88. uint8_t *data;
  89. uint32_t length;
  90. struct sb_command payload;
  91. struct sb_command c_payload;
  92. };
  93. struct sb_section_ctx {
  94. uint32_t size;
  95. /* Section flags */
  96. unsigned int boot:1;
  97. struct sb_section_ctx *sect;
  98. struct sb_cmd_ctx *cmd_head;
  99. struct sb_cmd_ctx *cmd_tail;
  100. struct sb_sections_header payload;
  101. };
  102. struct sb_image_ctx {
  103. unsigned int in_section:1;
  104. unsigned int in_dcd:1;
  105. /* Image configuration */
  106. unsigned int display_progress:1;
  107. unsigned int silent_dump:1;
  108. char *input_filename;
  109. char *output_filename;
  110. char *cfg_filename;
  111. uint8_t image_key[16];
  112. /* Number of section in the image */
  113. unsigned int sect_count;
  114. /* Bootable section */
  115. unsigned int sect_boot;
  116. unsigned int sect_boot_found:1;
  117. struct sb_section_ctx *sect_head;
  118. struct sb_section_ctx *sect_tail;
  119. struct sb_dcd_ctx *dcd_head;
  120. struct sb_dcd_ctx *dcd_tail;
  121. EVP_CIPHER_CTX cipher_ctx;
  122. EVP_MD_CTX md_ctx;
  123. uint8_t digest[32];
  124. struct sb_key_dictionary_key sb_dict_key;
  125. struct sb_boot_image_header payload;
  126. };
  127. /*
  128. * Instruction semantics:
  129. * NOOP
  130. * TAG [LAST]
  131. * LOAD address file
  132. * LOAD IVT address IVT_entry_point
  133. * FILL address pattern length
  134. * JUMP [HAB] address [r0_arg]
  135. * CALL [HAB] address [r0_arg]
  136. * MODE mode
  137. * For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
  138. * JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
  139. * For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
  140. * JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
  141. */
  142. /*
  143. * AES libcrypto
  144. */
  145. static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
  146. {
  147. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  148. int ret;
  149. /* If there is no init vector, init vector is all zeroes. */
  150. if (!iv)
  151. iv = ictx->image_key;
  152. EVP_CIPHER_CTX_init(ctx);
  153. ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
  154. if (ret == 1)
  155. EVP_CIPHER_CTX_set_padding(ctx, 0);
  156. return ret;
  157. }
  158. static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
  159. uint8_t *out_data, int in_len)
  160. {
  161. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  162. int ret, outlen;
  163. uint8_t *outbuf;
  164. outbuf = malloc(in_len);
  165. if (!outbuf)
  166. return -ENOMEM;
  167. memset(outbuf, 0, sizeof(in_len));
  168. ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
  169. if (!ret) {
  170. ret = -EINVAL;
  171. goto err;
  172. }
  173. if (out_data)
  174. memcpy(out_data, outbuf, outlen);
  175. err:
  176. free(outbuf);
  177. return ret;
  178. }
  179. static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
  180. {
  181. return EVP_CIPHER_CTX_cleanup(ctx);
  182. }
  183. static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
  184. {
  185. int ret;
  186. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  187. struct sb_boot_image_header *sb_header = &ictx->payload;
  188. uint8_t *iv = sb_header->iv;
  189. ret = sb_aes_deinit(ctx);
  190. if (!ret)
  191. return ret;
  192. return sb_aes_init(ictx, iv, enc);
  193. }
  194. /*
  195. * Debug
  196. */
  197. static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
  198. {
  199. va_list ap;
  200. if (ictx->silent_dump)
  201. return;
  202. va_start(ap, fmt);
  203. vfprintf(stdout, fmt, ap);
  204. va_end(ap);
  205. }
  206. /*
  207. * Code
  208. */
  209. static time_t sb_get_timestamp(void)
  210. {
  211. struct tm time_2000 = {
  212. .tm_yday = 1, /* Jan. 1st */
  213. .tm_year = 100, /* 2000 */
  214. };
  215. time_t seconds_to_2000 = mktime(&time_2000);
  216. time_t seconds_to_now = time(NULL);
  217. return seconds_to_now - seconds_to_2000;
  218. }
  219. static int sb_get_time(time_t time, struct tm *tm)
  220. {
  221. struct tm time_2000 = {
  222. .tm_yday = 1, /* Jan. 1st */
  223. .tm_year = 0, /* 1900 */
  224. };
  225. const time_t seconds_to_2000 = mktime(&time_2000);
  226. const time_t seconds_to_now = seconds_to_2000 + time;
  227. struct tm *ret;
  228. ret = gmtime_r(&seconds_to_now, tm);
  229. return ret ? 0 : -EINVAL;
  230. }
  231. static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
  232. {
  233. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  234. struct sb_boot_image_header *sb_header = &ictx->payload;
  235. uint8_t *sb_header_ptr = (uint8_t *)sb_header;
  236. /* Encrypt the header, compute the digest. */
  237. sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
  238. EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
  239. }
  240. static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
  241. {
  242. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  243. struct sb_section_ctx *sctx = ictx->sect_head;
  244. struct sb_sections_header *shdr;
  245. uint8_t *sb_sections_header_ptr;
  246. const int size = sizeof(*shdr);
  247. while (sctx) {
  248. shdr = &sctx->payload;
  249. sb_sections_header_ptr = (uint8_t *)shdr;
  250. sb_aes_crypt(ictx, sb_sections_header_ptr,
  251. ictx->sb_dict_key.cbc_mac, size);
  252. EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
  253. sctx = sctx->sect;
  254. };
  255. }
  256. static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  257. {
  258. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  259. sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
  260. sizeof(ictx->sb_dict_key.key));
  261. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  262. }
  263. static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  264. {
  265. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  266. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  267. sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
  268. sizeof(ictx->sb_dict_key.key));
  269. }
  270. static void sb_encrypt_tag(struct sb_image_ctx *ictx,
  271. struct sb_cmd_ctx *cctx)
  272. {
  273. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  274. struct sb_command *cmd = &cctx->payload;
  275. sb_aes_crypt(ictx, (uint8_t *)cmd,
  276. (uint8_t *)&cctx->c_payload, sizeof(*cmd));
  277. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  278. }
  279. static int sb_encrypt_image(struct sb_image_ctx *ictx)
  280. {
  281. /* Start image-wide crypto. */
  282. EVP_MD_CTX_init(&ictx->md_ctx);
  283. EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
  284. /*
  285. * SB image header.
  286. */
  287. sb_aes_init(ictx, NULL, 1);
  288. sb_encrypt_sb_header(ictx);
  289. /*
  290. * SB sections header.
  291. */
  292. sb_encrypt_sb_sections_header(ictx);
  293. /*
  294. * Key dictionary.
  295. */
  296. sb_aes_reinit(ictx, 1);
  297. sb_encrypt_key_dictionary_key(ictx);
  298. /*
  299. * Section tags.
  300. */
  301. struct sb_cmd_ctx *cctx;
  302. struct sb_command *ccmd;
  303. struct sb_section_ctx *sctx = ictx->sect_head;
  304. while (sctx) {
  305. cctx = sctx->cmd_head;
  306. sb_aes_reinit(ictx, 1);
  307. while (cctx) {
  308. ccmd = &cctx->payload;
  309. sb_encrypt_tag(ictx, cctx);
  310. if (ccmd->header.tag == ROM_TAG_CMD) {
  311. sb_aes_reinit(ictx, 1);
  312. } else if (ccmd->header.tag == ROM_LOAD_CMD) {
  313. sb_aes_crypt(ictx, cctx->data, cctx->data,
  314. cctx->length);
  315. EVP_DigestUpdate(&ictx->md_ctx, cctx->data,
  316. cctx->length);
  317. }
  318. cctx = cctx->cmd;
  319. }
  320. sctx = sctx->sect;
  321. };
  322. /*
  323. * Dump the SHA1 of the whole image.
  324. */
  325. sb_aes_reinit(ictx, 1);
  326. EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
  327. sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
  328. /* Stop the encryption session. */
  329. sb_aes_deinit(&ictx->cipher_ctx);
  330. return 0;
  331. }
  332. static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
  333. {
  334. long real_size, roundup_size;
  335. uint8_t *data;
  336. long ret;
  337. unsigned long size;
  338. FILE *fp;
  339. if (!filename) {
  340. fprintf(stderr, "ERR: Missing filename!\n");
  341. return -EINVAL;
  342. }
  343. fp = fopen(filename, "r");
  344. if (!fp)
  345. goto err_open;
  346. ret = fseek(fp, 0, SEEK_END);
  347. if (ret < 0)
  348. goto err_file;
  349. real_size = ftell(fp);
  350. if (real_size < 0)
  351. goto err_file;
  352. ret = fseek(fp, 0, SEEK_SET);
  353. if (ret < 0)
  354. goto err_file;
  355. roundup_size = roundup(real_size, SB_BLOCK_SIZE);
  356. data = calloc(1, roundup_size);
  357. if (!data)
  358. goto err_file;
  359. size = fread(data, 1, real_size, fp);
  360. if (size != (unsigned long)real_size)
  361. goto err_alloc;
  362. cctx->data = data;
  363. cctx->length = roundup_size;
  364. fclose(fp);
  365. return 0;
  366. err_alloc:
  367. free(data);
  368. err_file:
  369. fclose(fp);
  370. err_open:
  371. fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
  372. return -EINVAL;
  373. }
  374. static uint8_t sb_command_checksum(struct sb_command *inst)
  375. {
  376. uint8_t *inst_ptr = (uint8_t *)inst;
  377. uint8_t csum = 0;
  378. unsigned int i;
  379. for (i = 0; i < sizeof(struct sb_command); i++)
  380. csum += inst_ptr[i];
  381. return csum;
  382. }
  383. static int sb_token_to_long(char *tok, uint32_t *rid)
  384. {
  385. char *endptr;
  386. unsigned long id;
  387. if (tok[0] != '0' || tok[1] != 'x') {
  388. fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
  389. return -EINVAL;
  390. }
  391. tok += 2;
  392. errno = 0;
  393. id = strtoul(tok, &endptr, 16);
  394. if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
  395. fprintf(stderr, "ERR: Value can't be decoded!\n");
  396. return -EINVAL;
  397. }
  398. /* Check for 32-bit overflow. */
  399. if (id > 0xffffffff) {
  400. fprintf(stderr, "ERR: Value too big!\n");
  401. return -EINVAL;
  402. }
  403. if (endptr == tok) {
  404. fprintf(stderr, "ERR: Deformed value!\n");
  405. return -EINVAL;
  406. }
  407. *rid = (uint32_t)id;
  408. return 0;
  409. }
  410. static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
  411. {
  412. uint32_t *tmp;
  413. if (!inc_size)
  414. return 0;
  415. dctx->size += inc_size;
  416. tmp = realloc(dctx->payload, dctx->size);
  417. if (!tmp)
  418. return -ENOMEM;
  419. dctx->payload = tmp;
  420. /* Assemble and update the HAB DCD header. */
  421. dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
  422. (dctx->size << 8) |
  423. SB_HAB_VERSION);
  424. return 0;
  425. }
  426. static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  427. {
  428. struct sb_dcd_ctx *dctx;
  429. char *tok;
  430. uint32_t id;
  431. int ret;
  432. dctx = calloc(1, sizeof(*dctx));
  433. if (!dctx)
  434. return -ENOMEM;
  435. ret = sb_grow_dcd(dctx, 4);
  436. if (ret)
  437. goto err_dcd;
  438. /* Read DCD block number. */
  439. tok = strtok(cmd->cmd, " ");
  440. if (!tok) {
  441. fprintf(stderr, "#%i ERR: DCD block without number!\n",
  442. cmd->lineno);
  443. ret = -EINVAL;
  444. goto err_dcd;
  445. }
  446. /* Parse the DCD block number. */
  447. ret = sb_token_to_long(tok, &id);
  448. if (ret) {
  449. fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
  450. cmd->lineno);
  451. goto err_dcd;
  452. }
  453. dctx->id = id;
  454. /*
  455. * The DCD block is now constructed. Append it to the list.
  456. * WARNING: The DCD size is still not computed and will be
  457. * updated while parsing it's commands.
  458. */
  459. if (!ictx->dcd_head) {
  460. ictx->dcd_head = dctx;
  461. ictx->dcd_tail = dctx;
  462. } else {
  463. ictx->dcd_tail->dcd = dctx;
  464. ictx->dcd_tail = dctx;
  465. }
  466. return 0;
  467. err_dcd:
  468. free(dctx->payload);
  469. free(dctx);
  470. return ret;
  471. }
  472. static int sb_build_dcd_block(struct sb_image_ctx *ictx,
  473. struct sb_cmd_list *cmd,
  474. uint32_t type)
  475. {
  476. char *tok;
  477. uint32_t address, value, length;
  478. int ret;
  479. struct sb_dcd_ctx *dctx = ictx->dcd_tail;
  480. uint32_t *dcd;
  481. if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
  482. ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
  483. /* Same instruction as before, just append it. */
  484. ret = sb_grow_dcd(dctx, 8);
  485. if (ret)
  486. return ret;
  487. } else if (type == SB_DCD_NOOP) {
  488. ret = sb_grow_dcd(dctx, 4);
  489. if (ret)
  490. return ret;
  491. /* Update DCD command block pointer. */
  492. dctx->prev_dcd_head = dctx->payload +
  493. dctx->size / sizeof(*dctx->payload) - 1;
  494. /* NOOP has only 4 bytes and no payload. */
  495. goto noop;
  496. } else {
  497. /*
  498. * Either a different instruction block started now
  499. * or this is the first instruction block.
  500. */
  501. ret = sb_grow_dcd(dctx, 12);
  502. if (ret)
  503. return ret;
  504. /* Update DCD command block pointer. */
  505. dctx->prev_dcd_head = dctx->payload +
  506. dctx->size / sizeof(*dctx->payload) - 3;
  507. }
  508. dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;
  509. /*
  510. * Prepare the command.
  511. */
  512. tok = strtok(cmd->cmd, " ");
  513. if (!tok) {
  514. fprintf(stderr, "#%i ERR: Missing DCD address!\n",
  515. cmd->lineno);
  516. ret = -EINVAL;
  517. goto err;
  518. }
  519. /* Read DCD destination address. */
  520. ret = sb_token_to_long(tok, &address);
  521. if (ret) {
  522. fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
  523. cmd->lineno);
  524. goto err;
  525. }
  526. tok = strtok(NULL, " ");
  527. if (!tok) {
  528. fprintf(stderr, "#%i ERR: Missing DCD value!\n",
  529. cmd->lineno);
  530. ret = -EINVAL;
  531. goto err;
  532. }
  533. /* Read DCD operation value. */
  534. ret = sb_token_to_long(tok, &value);
  535. if (ret) {
  536. fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
  537. cmd->lineno);
  538. goto err;
  539. }
  540. /* Fill in the new DCD entry. */
  541. dcd[0] = htonl(address);
  542. dcd[1] = htonl(value);
  543. noop:
  544. /* Update the DCD command block. */
  545. length = dctx->size -
  546. ((dctx->prev_dcd_head - dctx->payload) *
  547. sizeof(*dctx->payload));
  548. dctx->prev_dcd_head[0] = htonl(type | (length << 8));
  549. err:
  550. return ret;
  551. }
  552. static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  553. {
  554. struct sb_section_ctx *sctx;
  555. struct sb_sections_header *shdr;
  556. char *tok;
  557. uint32_t bootable = 0;
  558. uint32_t id;
  559. int ret;
  560. sctx = calloc(1, sizeof(*sctx));
  561. if (!sctx)
  562. return -ENOMEM;
  563. /* Read section number. */
  564. tok = strtok(cmd->cmd, " ");
  565. if (!tok) {
  566. fprintf(stderr, "#%i ERR: Section without number!\n",
  567. cmd->lineno);
  568. ret = -EINVAL;
  569. goto err_sect;
  570. }
  571. /* Parse the section number. */
  572. ret = sb_token_to_long(tok, &id);
  573. if (ret) {
  574. fprintf(stderr, "#%i ERR: Malformed section number!\n",
  575. cmd->lineno);
  576. goto err_sect;
  577. }
  578. /* Read section's BOOTABLE flag. */
  579. tok = strtok(NULL, " ");
  580. if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
  581. bootable = SB_SECTION_FLAG_BOOTABLE;
  582. sctx->boot = bootable;
  583. shdr = &sctx->payload;
  584. shdr->section_number = id;
  585. shdr->section_flags = bootable;
  586. /*
  587. * The section is now constructed. Append it to the list.
  588. * WARNING: The section size is still not computed and will
  589. * be updated while parsing it's commands.
  590. */
  591. ictx->sect_count++;
  592. /* Mark that this section is bootable one. */
  593. if (bootable) {
  594. if (ictx->sect_boot_found) {
  595. fprintf(stderr,
  596. "#%i WARN: Multiple bootable section!\n",
  597. cmd->lineno);
  598. } else {
  599. ictx->sect_boot = id;
  600. ictx->sect_boot_found = 1;
  601. }
  602. }
  603. if (!ictx->sect_head) {
  604. ictx->sect_head = sctx;
  605. ictx->sect_tail = sctx;
  606. } else {
  607. ictx->sect_tail->sect = sctx;
  608. ictx->sect_tail = sctx;
  609. }
  610. return 0;
  611. err_sect:
  612. free(sctx);
  613. return ret;
  614. }
  615. static int sb_build_command_nop(struct sb_image_ctx *ictx)
  616. {
  617. struct sb_section_ctx *sctx = ictx->sect_tail;
  618. struct sb_cmd_ctx *cctx;
  619. struct sb_command *ccmd;
  620. cctx = calloc(1, sizeof(*cctx));
  621. if (!cctx)
  622. return -ENOMEM;
  623. ccmd = &cctx->payload;
  624. /*
  625. * Construct the command.
  626. */
  627. ccmd->header.checksum = 0x5a;
  628. ccmd->header.tag = ROM_NOP_CMD;
  629. cctx->size = sizeof(*ccmd);
  630. /*
  631. * Append the command to the last section.
  632. */
  633. if (!sctx->cmd_head) {
  634. sctx->cmd_head = cctx;
  635. sctx->cmd_tail = cctx;
  636. } else {
  637. sctx->cmd_tail->cmd = cctx;
  638. sctx->cmd_tail = cctx;
  639. }
  640. return 0;
  641. }
  642. static int sb_build_command_tag(struct sb_image_ctx *ictx,
  643. struct sb_cmd_list *cmd)
  644. {
  645. struct sb_section_ctx *sctx = ictx->sect_tail;
  646. struct sb_cmd_ctx *cctx;
  647. struct sb_command *ccmd;
  648. char *tok;
  649. cctx = calloc(1, sizeof(*cctx));
  650. if (!cctx)
  651. return -ENOMEM;
  652. ccmd = &cctx->payload;
  653. /*
  654. * Prepare the command.
  655. */
  656. /* Check for the LAST keyword. */
  657. tok = strtok(cmd->cmd, " ");
  658. if (tok && !strcmp(tok, "LAST"))
  659. ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
  660. /*
  661. * Construct the command.
  662. */
  663. ccmd->header.checksum = 0x5a;
  664. ccmd->header.tag = ROM_TAG_CMD;
  665. cctx->size = sizeof(*ccmd);
  666. /*
  667. * Append the command to the last section.
  668. */
  669. if (!sctx->cmd_head) {
  670. sctx->cmd_head = cctx;
  671. sctx->cmd_tail = cctx;
  672. } else {
  673. sctx->cmd_tail->cmd = cctx;
  674. sctx->cmd_tail = cctx;
  675. }
  676. return 0;
  677. }
  678. static int sb_build_command_load(struct sb_image_ctx *ictx,
  679. struct sb_cmd_list *cmd)
  680. {
  681. struct sb_section_ctx *sctx = ictx->sect_tail;
  682. struct sb_cmd_ctx *cctx;
  683. struct sb_command *ccmd;
  684. char *tok;
  685. int ret, is_ivt = 0, is_dcd = 0;
  686. uint32_t dest, dcd = 0;
  687. cctx = calloc(1, sizeof(*cctx));
  688. if (!cctx)
  689. return -ENOMEM;
  690. ccmd = &cctx->payload;
  691. /*
  692. * Prepare the command.
  693. */
  694. tok = strtok(cmd->cmd, " ");
  695. if (!tok) {
  696. fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
  697. cmd->lineno);
  698. ret = -EINVAL;
  699. goto err;
  700. }
  701. /* Check for "IVT" flag. */
  702. if (!strcmp(tok, "IVT"))
  703. is_ivt = 1;
  704. if (!strcmp(tok, "DCD"))
  705. is_dcd = 1;
  706. if (is_ivt || is_dcd) {
  707. tok = strtok(NULL, " ");
  708. if (!tok) {
  709. fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
  710. cmd->lineno);
  711. ret = -EINVAL;
  712. goto err;
  713. }
  714. }
  715. /* Read load destination address. */
  716. ret = sb_token_to_long(tok, &dest);
  717. if (ret) {
  718. fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
  719. cmd->lineno);
  720. goto err;
  721. }
  722. /* Read filename or IVT entrypoint or DCD block ID. */
  723. tok = strtok(NULL, " ");
  724. if (!tok) {
  725. fprintf(stderr,
  726. "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
  727. cmd->lineno);
  728. ret = -EINVAL;
  729. goto err;
  730. }
  731. if (is_ivt) {
  732. /* Handle IVT. */
  733. struct sb_ivt_header *ivt;
  734. uint32_t ivtep;
  735. ret = sb_token_to_long(tok, &ivtep);
  736. if (ret) {
  737. fprintf(stderr,
  738. "#%i ERR: Incorrect IVT entry point!\n",
  739. cmd->lineno);
  740. goto err;
  741. }
  742. ivt = calloc(1, sizeof(*ivt));
  743. if (!ivt) {
  744. ret = -ENOMEM;
  745. goto err;
  746. }
  747. ivt->header = sb_hab_ivt_header();
  748. ivt->entry = ivtep;
  749. ivt->self = dest;
  750. cctx->data = (uint8_t *)ivt;
  751. cctx->length = sizeof(*ivt);
  752. } else if (is_dcd) {
  753. struct sb_dcd_ctx *dctx = ictx->dcd_head;
  754. uint32_t dcdid;
  755. uint8_t *payload;
  756. uint32_t asize;
  757. ret = sb_token_to_long(tok, &dcdid);
  758. if (ret) {
  759. fprintf(stderr,
  760. "#%i ERR: Incorrect DCD block ID!\n",
  761. cmd->lineno);
  762. goto err;
  763. }
  764. while (dctx) {
  765. if (dctx->id == dcdid)
  766. break;
  767. dctx = dctx->dcd;
  768. }
  769. if (!dctx) {
  770. fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
  771. cmd->lineno, dcdid);
  772. goto err;
  773. }
  774. asize = roundup(dctx->size, SB_BLOCK_SIZE);
  775. payload = calloc(1, asize);
  776. if (!payload) {
  777. ret = -ENOMEM;
  778. goto err;
  779. }
  780. memcpy(payload, dctx->payload, dctx->size);
  781. cctx->data = payload;
  782. cctx->length = asize;
  783. /* Set the Load DCD flag. */
  784. dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
  785. } else {
  786. /* Regular LOAD of a file. */
  787. ret = sb_load_file(cctx, tok);
  788. if (ret) {
  789. fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
  790. cmd->lineno, tok);
  791. goto err;
  792. }
  793. }
  794. if (cctx->length & (SB_BLOCK_SIZE - 1)) {
  795. fprintf(stderr, "#%i ERR: Unaligned payload!\n",
  796. cmd->lineno);
  797. }
  798. /*
  799. * Construct the command.
  800. */
  801. ccmd->header.checksum = 0x5a;
  802. ccmd->header.tag = ROM_LOAD_CMD;
  803. ccmd->header.flags = dcd;
  804. ccmd->load.address = dest;
  805. ccmd->load.count = cctx->length;
  806. ccmd->load.crc32 = pbl_crc32(0,
  807. (const char *)cctx->data,
  808. cctx->length);
  809. cctx->size = sizeof(*ccmd) + cctx->length;
  810. /*
  811. * Append the command to the last section.
  812. */
  813. if (!sctx->cmd_head) {
  814. sctx->cmd_head = cctx;
  815. sctx->cmd_tail = cctx;
  816. } else {
  817. sctx->cmd_tail->cmd = cctx;
  818. sctx->cmd_tail = cctx;
  819. }
  820. return 0;
  821. err:
  822. free(cctx);
  823. return ret;
  824. }
  825. static int sb_build_command_fill(struct sb_image_ctx *ictx,
  826. struct sb_cmd_list *cmd)
  827. {
  828. struct sb_section_ctx *sctx = ictx->sect_tail;
  829. struct sb_cmd_ctx *cctx;
  830. struct sb_command *ccmd;
  831. char *tok;
  832. uint32_t address, pattern, length;
  833. int ret;
  834. cctx = calloc(1, sizeof(*cctx));
  835. if (!cctx)
  836. return -ENOMEM;
  837. ccmd = &cctx->payload;
  838. /*
  839. * Prepare the command.
  840. */
  841. tok = strtok(cmd->cmd, " ");
  842. if (!tok) {
  843. fprintf(stderr, "#%i ERR: Missing FILL address!\n",
  844. cmd->lineno);
  845. ret = -EINVAL;
  846. goto err;
  847. }
  848. /* Read fill destination address. */
  849. ret = sb_token_to_long(tok, &address);
  850. if (ret) {
  851. fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
  852. cmd->lineno);
  853. goto err;
  854. }
  855. tok = strtok(NULL, " ");
  856. if (!tok) {
  857. fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
  858. cmd->lineno);
  859. ret = -EINVAL;
  860. goto err;
  861. }
  862. /* Read fill pattern address. */
  863. ret = sb_token_to_long(tok, &pattern);
  864. if (ret) {
  865. fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
  866. cmd->lineno);
  867. goto err;
  868. }
  869. tok = strtok(NULL, " ");
  870. if (!tok) {
  871. fprintf(stderr, "#%i ERR: Missing FILL length!\n",
  872. cmd->lineno);
  873. ret = -EINVAL;
  874. goto err;
  875. }
  876. /* Read fill pattern address. */
  877. ret = sb_token_to_long(tok, &length);
  878. if (ret) {
  879. fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
  880. cmd->lineno);
  881. goto err;
  882. }
  883. /*
  884. * Construct the command.
  885. */
  886. ccmd->header.checksum = 0x5a;
  887. ccmd->header.tag = ROM_FILL_CMD;
  888. ccmd->fill.address = address;
  889. ccmd->fill.count = length;
  890. ccmd->fill.pattern = pattern;
  891. cctx->size = sizeof(*ccmd);
  892. /*
  893. * Append the command to the last section.
  894. */
  895. if (!sctx->cmd_head) {
  896. sctx->cmd_head = cctx;
  897. sctx->cmd_tail = cctx;
  898. } else {
  899. sctx->cmd_tail->cmd = cctx;
  900. sctx->cmd_tail = cctx;
  901. }
  902. return 0;
  903. err:
  904. free(cctx);
  905. return ret;
  906. }
  907. static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
  908. struct sb_cmd_list *cmd,
  909. unsigned int is_call)
  910. {
  911. struct sb_section_ctx *sctx = ictx->sect_tail;
  912. struct sb_cmd_ctx *cctx;
  913. struct sb_command *ccmd;
  914. char *tok;
  915. uint32_t dest, arg = 0x0;
  916. uint32_t hab = 0;
  917. int ret;
  918. const char *cmdname = is_call ? "CALL" : "JUMP";
  919. cctx = calloc(1, sizeof(*cctx));
  920. if (!cctx)
  921. return -ENOMEM;
  922. ccmd = &cctx->payload;
  923. /*
  924. * Prepare the command.
  925. */
  926. tok = strtok(cmd->cmd, " ");
  927. if (!tok) {
  928. fprintf(stderr,
  929. "#%i ERR: Missing %s address or 'HAB'!\n",
  930. cmd->lineno, cmdname);
  931. ret = -EINVAL;
  932. goto err;
  933. }
  934. /* Check for "HAB" flag. */
  935. if (!strcmp(tok, "HAB")) {
  936. hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
  937. tok = strtok(NULL, " ");
  938. if (!tok) {
  939. fprintf(stderr, "#%i ERR: Missing %s address!\n",
  940. cmd->lineno, cmdname);
  941. ret = -EINVAL;
  942. goto err;
  943. }
  944. }
  945. /* Read load destination address. */
  946. ret = sb_token_to_long(tok, &dest);
  947. if (ret) {
  948. fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
  949. cmd->lineno, cmdname);
  950. goto err;
  951. }
  952. tok = strtok(NULL, " ");
  953. if (tok) {
  954. ret = sb_token_to_long(tok, &arg);
  955. if (ret) {
  956. fprintf(stderr,
  957. "#%i ERR: Incorrect %s argument!\n",
  958. cmd->lineno, cmdname);
  959. goto err;
  960. }
  961. }
  962. /*
  963. * Construct the command.
  964. */
  965. ccmd->header.checksum = 0x5a;
  966. ccmd->header.tag = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
  967. ccmd->header.flags = hab;
  968. ccmd->call.address = dest;
  969. ccmd->call.argument = arg;
  970. cctx->size = sizeof(*ccmd);
  971. /*
  972. * Append the command to the last section.
  973. */
  974. if (!sctx->cmd_head) {
  975. sctx->cmd_head = cctx;
  976. sctx->cmd_tail = cctx;
  977. } else {
  978. sctx->cmd_tail->cmd = cctx;
  979. sctx->cmd_tail = cctx;
  980. }
  981. return 0;
  982. err:
  983. free(cctx);
  984. return ret;
  985. }
  986. static int sb_build_command_jump(struct sb_image_ctx *ictx,
  987. struct sb_cmd_list *cmd)
  988. {
  989. return sb_build_command_jump_call(ictx, cmd, 0);
  990. }
  991. static int sb_build_command_call(struct sb_image_ctx *ictx,
  992. struct sb_cmd_list *cmd)
  993. {
  994. return sb_build_command_jump_call(ictx, cmd, 1);
  995. }
  996. static int sb_build_command_mode(struct sb_image_ctx *ictx,
  997. struct sb_cmd_list *cmd)
  998. {
  999. struct sb_section_ctx *sctx = ictx->sect_tail;
  1000. struct sb_cmd_ctx *cctx;
  1001. struct sb_command *ccmd;
  1002. char *tok;
  1003. int ret;
  1004. unsigned int i;
  1005. uint32_t mode = 0xffffffff;
  1006. cctx = calloc(1, sizeof(*cctx));
  1007. if (!cctx)
  1008. return -ENOMEM;
  1009. ccmd = &cctx->payload;
  1010. /*
  1011. * Prepare the command.
  1012. */
  1013. tok = strtok(cmd->cmd, " ");
  1014. if (!tok) {
  1015. fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
  1016. cmd->lineno);
  1017. ret = -EINVAL;
  1018. goto err;
  1019. }
  1020. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1021. if (!strcmp(tok, modetable[i].name)) {
  1022. mode = modetable[i].mode;
  1023. break;
  1024. }
  1025. if (!modetable[i].altname)
  1026. continue;
  1027. if (!strcmp(tok, modetable[i].altname)) {
  1028. mode = modetable[i].mode;
  1029. break;
  1030. }
  1031. }
  1032. if (mode == 0xffffffff) {
  1033. fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
  1034. cmd->lineno);
  1035. ret = -EINVAL;
  1036. goto err;
  1037. }
  1038. /*
  1039. * Construct the command.
  1040. */
  1041. ccmd->header.checksum = 0x5a;
  1042. ccmd->header.tag = ROM_MODE_CMD;
  1043. ccmd->mode.mode = mode;
  1044. cctx->size = sizeof(*ccmd);
  1045. /*
  1046. * Append the command to the last section.
  1047. */
  1048. if (!sctx->cmd_head) {
  1049. sctx->cmd_head = cctx;
  1050. sctx->cmd_tail = cctx;
  1051. } else {
  1052. sctx->cmd_tail->cmd = cctx;
  1053. sctx->cmd_tail = cctx;
  1054. }
  1055. return 0;
  1056. err:
  1057. free(cctx);
  1058. return ret;
  1059. }
  1060. static int sb_prefill_image_header(struct sb_image_ctx *ictx)
  1061. {
  1062. struct sb_boot_image_header *hdr = &ictx->payload;
  1063. /* Fill signatures */
  1064. memcpy(hdr->signature1, "STMP", 4);
  1065. memcpy(hdr->signature2, "sgtl", 4);
  1066. /* SB Image version 1.1 */
  1067. hdr->major_version = SB_VERSION_MAJOR;
  1068. hdr->minor_version = SB_VERSION_MINOR;
  1069. /* Boot image major version */
  1070. hdr->product_version.major = htons(0x999);
  1071. hdr->product_version.minor = htons(0x999);
  1072. hdr->product_version.revision = htons(0x999);
  1073. /* Boot image major version */
  1074. hdr->component_version.major = htons(0x999);
  1075. hdr->component_version.minor = htons(0x999);
  1076. hdr->component_version.revision = htons(0x999);
  1077. /* Drive tag must be 0x0 for i.MX23 */
  1078. hdr->drive_tag = 0;
  1079. hdr->header_blocks =
  1080. sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1081. hdr->section_header_size =
  1082. sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1083. hdr->timestamp_us = sb_get_timestamp() * 1000000;
  1084. hdr->flags = ictx->display_progress ?
  1085. SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0;
  1086. /* FIXME -- We support only default key */
  1087. hdr->key_count = 1;
  1088. return 0;
  1089. }
  1090. static int sb_postfill_image_header(struct sb_image_ctx *ictx)
  1091. {
  1092. struct sb_boot_image_header *hdr = &ictx->payload;
  1093. struct sb_section_ctx *sctx = ictx->sect_head;
  1094. uint32_t kd_size, sections_blocks;
  1095. EVP_MD_CTX md_ctx;
  1096. /* The main SB header size in blocks. */
  1097. hdr->image_blocks = hdr->header_blocks;
  1098. /* Size of the key dictionary, which has single zero entry. */
  1099. kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
  1100. hdr->image_blocks += kd_size / SB_BLOCK_SIZE;
  1101. /* Now count the payloads. */
  1102. hdr->section_count = ictx->sect_count;
  1103. while (sctx) {
  1104. hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
  1105. sctx = sctx->sect;
  1106. }
  1107. if (!ictx->sect_boot_found) {
  1108. fprintf(stderr, "ERR: No bootable section selected!\n");
  1109. return -EINVAL;
  1110. }
  1111. hdr->first_boot_section_id = ictx->sect_boot;
  1112. /* The n * SB section size in blocks. */
  1113. sections_blocks = hdr->section_count * hdr->section_header_size;
  1114. hdr->image_blocks += sections_blocks;
  1115. /* Key dictionary offset. */
  1116. hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;
  1117. /* Digest of the whole image. */
  1118. hdr->image_blocks += 2;
  1119. /* Pointer past the dictionary. */
  1120. hdr->first_boot_tag_block =
  1121. hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;
  1122. /* Compute header digest. */
  1123. EVP_MD_CTX_init(&md_ctx);
  1124. EVP_DigestInit(&md_ctx, EVP_sha1());
  1125. EVP_DigestUpdate(&md_ctx, hdr->signature1,
  1126. sizeof(struct sb_boot_image_header) -
  1127. sizeof(hdr->digest));
  1128. EVP_DigestFinal(&md_ctx, hdr->digest, NULL);
  1129. return 0;
  1130. }
  1131. static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
  1132. {
  1133. /* Fixup the placement of sections. */
  1134. struct sb_boot_image_header *ihdr = &ictx->payload;
  1135. struct sb_section_ctx *sctx = ictx->sect_head;
  1136. struct sb_sections_header *shdr;
  1137. struct sb_cmd_ctx *cctx;
  1138. struct sb_command *ccmd;
  1139. uint32_t offset = ihdr->first_boot_tag_block;
  1140. while (sctx) {
  1141. shdr = &sctx->payload;
  1142. /* Fill in the section TAG offset. */
  1143. shdr->section_offset = offset + 1;
  1144. offset += shdr->section_size;
  1145. /* Section length is measured from the TAG block. */
  1146. shdr->section_size--;
  1147. /* Fixup the TAG command. */
  1148. cctx = sctx->cmd_head;
  1149. while (cctx) {
  1150. ccmd = &cctx->payload;
  1151. if (ccmd->header.tag == ROM_TAG_CMD) {
  1152. ccmd->tag.section_number = shdr->section_number;
  1153. ccmd->tag.section_length = shdr->section_size;
  1154. ccmd->tag.section_flags = shdr->section_flags;
  1155. }
  1156. /* Update the command checksum. */
  1157. ccmd->header.checksum = sb_command_checksum(ccmd);
  1158. cctx = cctx->cmd;
  1159. }
  1160. sctx = sctx->sect;
  1161. }
  1162. return 0;
  1163. }
  1164. static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  1165. {
  1166. char *tok;
  1167. char *line = cmd->cmd;
  1168. char *rptr;
  1169. int ret;
  1170. /* Analyze the identifier on this line first. */
  1171. tok = strtok_r(line, " ", &rptr);
  1172. if (!tok || (strlen(tok) == 0)) {
  1173. fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
  1174. return -EINVAL;
  1175. }
  1176. cmd->cmd = rptr;
  1177. /* set DISPLAY_PROGRESS flag */
  1178. if (!strcmp(tok, "DISPLAYPROGRESS")) {
  1179. ictx->display_progress = 1;
  1180. return 0;
  1181. }
  1182. /* DCD */
  1183. if (!strcmp(tok, "DCD")) {
  1184. ictx->in_section = 0;
  1185. ictx->in_dcd = 1;
  1186. sb_build_dcd(ictx, cmd);
  1187. return 0;
  1188. }
  1189. /* Section */
  1190. if (!strcmp(tok, "SECTION")) {
  1191. ictx->in_section = 1;
  1192. ictx->in_dcd = 0;
  1193. sb_build_section(ictx, cmd);
  1194. return 0;
  1195. }
  1196. if (!ictx->in_section && !ictx->in_dcd) {
  1197. fprintf(stderr, "#%i ERR: Data outside of a section!\n",
  1198. cmd->lineno);
  1199. return -EINVAL;
  1200. }
  1201. if (ictx->in_section) {
  1202. /* Section commands */
  1203. if (!strcmp(tok, "NOP")) {
  1204. ret = sb_build_command_nop(ictx);
  1205. } else if (!strcmp(tok, "TAG")) {
  1206. ret = sb_build_command_tag(ictx, cmd);
  1207. } else if (!strcmp(tok, "LOAD")) {
  1208. ret = sb_build_command_load(ictx, cmd);
  1209. } else if (!strcmp(tok, "FILL")) {
  1210. ret = sb_build_command_fill(ictx, cmd);
  1211. } else if (!strcmp(tok, "JUMP")) {
  1212. ret = sb_build_command_jump(ictx, cmd);
  1213. } else if (!strcmp(tok, "CALL")) {
  1214. ret = sb_build_command_call(ictx, cmd);
  1215. } else if (!strcmp(tok, "MODE")) {
  1216. ret = sb_build_command_mode(ictx, cmd);
  1217. } else {
  1218. fprintf(stderr,
  1219. "#%i ERR: Unsupported instruction '%s'!\n",
  1220. cmd->lineno, tok);
  1221. return -ENOTSUP;
  1222. }
  1223. } else if (ictx->in_dcd) {
  1224. char *lptr;
  1225. uint32_t ilen = '1';
  1226. tok = strtok_r(tok, ".", &lptr);
  1227. if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) {
  1228. fprintf(stderr, "#%i ERR: Invalid line!\n",
  1229. cmd->lineno);
  1230. return -EINVAL;
  1231. }
  1232. if (lptr &&
  1233. (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) {
  1234. fprintf(stderr, "#%i ERR: Invalid instruction width!\n",
  1235. cmd->lineno);
  1236. return -EINVAL;
  1237. }
  1238. if (lptr)
  1239. ilen = lptr[0] - '1';
  1240. /* DCD commands */
  1241. if (!strcmp(tok, "WRITE")) {
  1242. ret = sb_build_dcd_block(ictx, cmd,
  1243. SB_DCD_WRITE | ilen);
  1244. } else if (!strcmp(tok, "ANDC")) {
  1245. ret = sb_build_dcd_block(ictx, cmd,
  1246. SB_DCD_ANDC | ilen);
  1247. } else if (!strcmp(tok, "ORR")) {
  1248. ret = sb_build_dcd_block(ictx, cmd,
  1249. SB_DCD_ORR | ilen);
  1250. } else if (!strcmp(tok, "EQZ")) {
  1251. ret = sb_build_dcd_block(ictx, cmd,
  1252. SB_DCD_CHK_EQZ | ilen);
  1253. } else if (!strcmp(tok, "EQ")) {
  1254. ret = sb_build_dcd_block(ictx, cmd,
  1255. SB_DCD_CHK_EQ | ilen);
  1256. } else if (!strcmp(tok, "NEQ")) {
  1257. ret = sb_build_dcd_block(ictx, cmd,
  1258. SB_DCD_CHK_NEQ | ilen);
  1259. } else if (!strcmp(tok, "NEZ")) {
  1260. ret = sb_build_dcd_block(ictx, cmd,
  1261. SB_DCD_CHK_NEZ | ilen);
  1262. } else if (!strcmp(tok, "NOOP")) {
  1263. ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP);
  1264. } else {
  1265. fprintf(stderr,
  1266. "#%i ERR: Unsupported instruction '%s'!\n",
  1267. cmd->lineno, tok);
  1268. return -ENOTSUP;
  1269. }
  1270. } else {
  1271. fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
  1272. cmd->lineno, tok);
  1273. return -ENOTSUP;
  1274. }
  1275. /*
  1276. * Here we have at least one section with one command, otherwise we
  1277. * would have failed already higher above.
  1278. *
  1279. * FIXME -- should the updating happen here ?
  1280. */
  1281. if (ictx->in_section && !ret) {
  1282. ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
  1283. ictx->sect_tail->payload.section_size =
  1284. ictx->sect_tail->size / SB_BLOCK_SIZE;
  1285. }
  1286. return ret;
  1287. }
  1288. static int sb_load_cmdfile(struct sb_image_ctx *ictx)
  1289. {
  1290. struct sb_cmd_list cmd;
  1291. int lineno = 1;
  1292. FILE *fp;
  1293. char *line = NULL;
  1294. ssize_t rlen;
  1295. size_t len;
  1296. fp = fopen(ictx->cfg_filename, "r");
  1297. if (!fp)
  1298. goto err_file;
  1299. while ((rlen = getline(&line, &len, fp)) > 0) {
  1300. memset(&cmd, 0, sizeof(cmd));
  1301. /* Strip the trailing newline. */
  1302. line[rlen - 1] = '\0';
  1303. cmd.cmd = line;
  1304. cmd.len = rlen;
  1305. cmd.lineno = lineno++;
  1306. sb_parse_line(ictx, &cmd);
  1307. }
  1308. free(line);
  1309. fclose(fp);
  1310. return 0;
  1311. err_file:
  1312. fclose(fp);
  1313. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1314. ictx->cfg_filename);
  1315. return -EINVAL;
  1316. }
  1317. static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
  1318. {
  1319. int ret;
  1320. ret = sb_load_cmdfile(ictx);
  1321. if (ret)
  1322. return ret;
  1323. ret = sb_prefill_image_header(ictx);
  1324. if (ret)
  1325. return ret;
  1326. ret = sb_postfill_image_header(ictx);
  1327. if (ret)
  1328. return ret;
  1329. ret = sb_fixup_sections_and_tags(ictx);
  1330. if (ret)
  1331. return ret;
  1332. return 0;
  1333. }
  1334. static int sb_verify_image_header(struct sb_image_ctx *ictx,
  1335. FILE *fp, long fsize)
  1336. {
  1337. /* Verify static fields in the image header. */
  1338. struct sb_boot_image_header *hdr = &ictx->payload;
  1339. const char *stat[2] = { "[PASS]", "[FAIL]" };
  1340. struct tm tm;
  1341. int sz, ret = 0;
  1342. unsigned char digest[20];
  1343. EVP_MD_CTX md_ctx;
  1344. unsigned long size;
  1345. /* Start image-wide crypto. */
  1346. EVP_MD_CTX_init(&ictx->md_ctx);
  1347. EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
  1348. soprintf(ictx, "---------- Verifying SB Image Header ----------\n");
  1349. size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp);
  1350. if (size != sizeof(ictx->payload)) {
  1351. fprintf(stderr, "ERR: SB image header too short!\n");
  1352. return -EINVAL;
  1353. }
  1354. /* Compute header digest. */
  1355. EVP_MD_CTX_init(&md_ctx);
  1356. EVP_DigestInit(&md_ctx, EVP_sha1());
  1357. EVP_DigestUpdate(&md_ctx, hdr->signature1,
  1358. sizeof(struct sb_boot_image_header) -
  1359. sizeof(hdr->digest));
  1360. EVP_DigestFinal(&md_ctx, digest, NULL);
  1361. sb_aes_init(ictx, NULL, 1);
  1362. sb_encrypt_sb_header(ictx);
  1363. if (memcmp(digest, hdr->digest, 20))
  1364. ret = -EINVAL;
  1365. soprintf(ictx, "%s Image header checksum: %s\n", stat[!!ret],
  1366. ret ? "BAD" : "OK");
  1367. if (ret)
  1368. return ret;
  1369. if (memcmp(hdr->signature1, "STMP", 4) ||
  1370. memcmp(hdr->signature2, "sgtl", 4))
  1371. ret = -EINVAL;
  1372. soprintf(ictx, "%s Signatures: '%.4s' '%.4s'\n",
  1373. stat[!!ret], hdr->signature1, hdr->signature2);
  1374. if (ret)
  1375. return ret;
  1376. if ((hdr->major_version != SB_VERSION_MAJOR) ||
  1377. ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
  1378. ret = -EINVAL;
  1379. soprintf(ictx, "%s Image version: v%i.%i\n", stat[!!ret],
  1380. hdr->major_version, hdr->minor_version);
  1381. if (ret)
  1382. return ret;
  1383. ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
  1384. soprintf(ictx,
  1385. "%s Creation time: %02i:%02i:%02i %02i/%02i/%04i\n",
  1386. stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
  1387. tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
  1388. if (ret)
  1389. return ret;
  1390. soprintf(ictx, "%s Product version: %x.%x.%x\n", stat[0],
  1391. ntohs(hdr->product_version.major),
  1392. ntohs(hdr->product_version.minor),
  1393. ntohs(hdr->product_version.revision));
  1394. soprintf(ictx, "%s Component version: %x.%x.%x\n", stat[0],
  1395. ntohs(hdr->component_version.major),
  1396. ntohs(hdr->component_version.minor),
  1397. ntohs(hdr->component_version.revision));
  1398. if (hdr->flags & ~SB_IMAGE_FLAGS_MASK)
  1399. ret = -EINVAL;
  1400. soprintf(ictx, "%s Image flags: %s\n", stat[!!ret],
  1401. hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ?
  1402. "Display_progress" : "");
  1403. if (ret)
  1404. return ret;
  1405. if (hdr->drive_tag != 0)
  1406. ret = -EINVAL;
  1407. soprintf(ictx, "%s Drive tag: %i\n", stat[!!ret],
  1408. hdr->drive_tag);
  1409. if (ret)
  1410. return ret;
  1411. sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1412. if (hdr->header_blocks != sz)
  1413. ret = -EINVAL;
  1414. soprintf(ictx, "%s Image header size (blocks): %i\n", stat[!!ret],
  1415. hdr->header_blocks);
  1416. if (ret)
  1417. return ret;
  1418. sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1419. if (hdr->section_header_size != sz)
  1420. ret = -EINVAL;
  1421. soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret],
  1422. hdr->section_header_size);
  1423. if (ret)
  1424. return ret;
  1425. soprintf(ictx, "%s Sections count: %i\n", stat[!!ret],
  1426. hdr->section_count);
  1427. soprintf(ictx, "%s First bootable section %i\n", stat[!!ret],
  1428. hdr->first_boot_section_id);
  1429. if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)
  1430. ret = -EINVAL;
  1431. soprintf(ictx, "%s Image size (blocks): %i\n", stat[!!ret],
  1432. hdr->image_blocks);
  1433. if (ret)
  1434. return ret;
  1435. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1436. if (hdr->key_dictionary_block != sz)
  1437. ret = -EINVAL;
  1438. soprintf(ictx, "%s Key dict offset (blocks): %i\n", stat[!!ret],
  1439. hdr->key_dictionary_block);
  1440. if (ret)
  1441. return ret;
  1442. if (hdr->key_count != 1)
  1443. ret = -EINVAL;
  1444. soprintf(ictx, "%s Number of encryption keys: %i\n", stat[!!ret],
  1445. hdr->key_count);
  1446. if (ret)
  1447. return ret;
  1448. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1449. sz += hdr->key_count *
  1450. sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE;
  1451. if (hdr->first_boot_tag_block != (unsigned)sz)
  1452. ret = -EINVAL;
  1453. soprintf(ictx, "%s First TAG block (blocks): %i\n", stat[!!ret],
  1454. hdr->first_boot_tag_block);
  1455. if (ret)
  1456. return ret;
  1457. return 0;
  1458. }
  1459. static void sb_decrypt_tag(struct sb_image_ctx *ictx,
  1460. struct sb_cmd_ctx *cctx)
  1461. {
  1462. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  1463. struct sb_command *cmd = &cctx->payload;
  1464. sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload,
  1465. (uint8_t *)&cctx->payload, sizeof(*cmd));
  1466. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  1467. }
  1468. static int sb_verify_command(struct sb_image_ctx *ictx,
  1469. struct sb_cmd_ctx *cctx, FILE *fp,
  1470. unsigned long *tsize)
  1471. {
  1472. struct sb_command *ccmd = &cctx->payload;
  1473. unsigned long size, asize;
  1474. char *csum, *flag = "";
  1475. int ret;
  1476. unsigned int i;
  1477. uint8_t csn, csc = ccmd->header.checksum;
  1478. ccmd->header.checksum = 0x5a;
  1479. csn = sb_command_checksum(ccmd);
  1480. ccmd->header.checksum = csc;
  1481. if (csc == csn)
  1482. ret = 0;
  1483. else
  1484. ret = -EINVAL;
  1485. csum = ret ? "checksum BAD" : "checksum OK";
  1486. switch (ccmd->header.tag) {
  1487. case ROM_NOP_CMD:
  1488. soprintf(ictx, " NOOP # %s\n", csum);
  1489. return ret;
  1490. case ROM_TAG_CMD:
  1491. if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
  1492. flag = "LAST";
  1493. soprintf(ictx, " TAG %s # %s\n", flag, csum);
  1494. sb_aes_reinit(ictx, 0);
  1495. return ret;
  1496. case ROM_LOAD_CMD:
  1497. soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
  1498. ccmd->load.address, ccmd->load.count, csum);
  1499. cctx->length = ccmd->load.count;
  1500. asize = roundup(cctx->length, SB_BLOCK_SIZE);
  1501. cctx->data = malloc(asize);
  1502. if (!cctx->data)
  1503. return -ENOMEM;
  1504. size = fread(cctx->data, 1, asize, fp);
  1505. if (size != asize) {
  1506. fprintf(stderr,
  1507. "ERR: SB LOAD command payload too short!\n");
  1508. return -EINVAL;
  1509. }
  1510. *tsize += size;
  1511. EVP_DigestUpdate(&ictx->md_ctx, cctx->data, asize);
  1512. sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
  1513. if (ccmd->load.crc32 != pbl_crc32(0,
  1514. (const char *)cctx->data,
  1515. asize)) {
  1516. fprintf(stderr,
  1517. "ERR: SB LOAD command payload CRC32 invalid!\n");
  1518. return -EINVAL;
  1519. }
  1520. return 0;
  1521. case ROM_FILL_CMD:
  1522. soprintf(ictx,
  1523. " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
  1524. ccmd->fill.address, ccmd->fill.count,
  1525. ccmd->fill.pattern, csum);
  1526. return 0;
  1527. case ROM_JUMP_CMD:
  1528. if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
  1529. flag = " HAB";
  1530. soprintf(ictx,
  1531. " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1532. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1533. return 0;
  1534. case ROM_CALL_CMD:
  1535. if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
  1536. flag = " HAB";
  1537. soprintf(ictx,
  1538. " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1539. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1540. return 0;
  1541. case ROM_MODE_CMD:
  1542. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1543. if (ccmd->mode.mode == modetable[i].mode) {
  1544. soprintf(ictx, " MODE %s # %s\n",
  1545. modetable[i].name, csum);
  1546. break;
  1547. }
  1548. }
  1549. fprintf(stderr, " MODE !INVALID! # %s\n", csum);
  1550. return 0;
  1551. }
  1552. return ret;
  1553. }
  1554. static int sb_verify_commands(struct sb_image_ctx *ictx,
  1555. struct sb_section_ctx *sctx, FILE *fp)
  1556. {
  1557. unsigned long size, tsize = 0;
  1558. struct sb_cmd_ctx *cctx;
  1559. int ret;
  1560. sb_aes_reinit(ictx, 0);
  1561. while (tsize < sctx->size) {
  1562. cctx = calloc(1, sizeof(*cctx));
  1563. if (!cctx)
  1564. return -ENOMEM;
  1565. if (!sctx->cmd_head) {
  1566. sctx->cmd_head = cctx;
  1567. sctx->cmd_tail = cctx;
  1568. } else {
  1569. sctx->cmd_tail->cmd = cctx;
  1570. sctx->cmd_tail = cctx;
  1571. }
  1572. size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp);
  1573. if (size != sizeof(cctx->c_payload)) {
  1574. fprintf(stderr, "ERR: SB command header too short!\n");
  1575. return -EINVAL;
  1576. }
  1577. tsize += size;
  1578. sb_decrypt_tag(ictx, cctx);
  1579. ret = sb_verify_command(ictx, cctx, fp, &tsize);
  1580. if (ret)
  1581. return -EINVAL;
  1582. }
  1583. return 0;
  1584. }
  1585. static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp)
  1586. {
  1587. struct sb_boot_image_header *hdr = &ictx->payload;
  1588. struct sb_sections_header *shdr;
  1589. unsigned int i;
  1590. int ret;
  1591. struct sb_section_ctx *sctx;
  1592. unsigned long size;
  1593. char *bootable = "";
  1594. soprintf(ictx, "----- Verifying SB Sections and Commands -----\n");
  1595. for (i = 0; i < hdr->section_count; i++) {
  1596. sctx = calloc(1, sizeof(*sctx));
  1597. if (!sctx)
  1598. return -ENOMEM;
  1599. if (!ictx->sect_head) {
  1600. ictx->sect_head = sctx;
  1601. ictx->sect_tail = sctx;
  1602. } else {
  1603. ictx->sect_tail->sect = sctx;
  1604. ictx->sect_tail = sctx;
  1605. }
  1606. size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp);
  1607. if (size != sizeof(sctx->payload)) {
  1608. fprintf(stderr, "ERR: SB section header too short!\n");
  1609. return -EINVAL;
  1610. }
  1611. }
  1612. size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp);
  1613. if (size != sizeof(ictx->sb_dict_key)) {
  1614. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1615. return -EINVAL;
  1616. }
  1617. sb_encrypt_sb_sections_header(ictx);
  1618. sb_aes_reinit(ictx, 0);
  1619. sb_decrypt_key_dictionary_key(ictx);
  1620. sb_aes_reinit(ictx, 0);
  1621. sctx = ictx->sect_head;
  1622. while (sctx) {
  1623. shdr = &sctx->payload;
  1624. if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) {
  1625. sctx->boot = 1;
  1626. bootable = " BOOTABLE";
  1627. }
  1628. sctx->size = (shdr->section_size * SB_BLOCK_SIZE) +
  1629. sizeof(struct sb_command);
  1630. soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n",
  1631. shdr->section_number, bootable, sctx->size);
  1632. if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE)
  1633. fprintf(stderr, " WARN: Unknown section flag(s) %08x\n",
  1634. shdr->section_flags);
  1635. if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) &&
  1636. (hdr->first_boot_section_id != shdr->section_number)) {
  1637. fprintf(stderr,
  1638. " WARN: Bootable section does ID not match image header ID!\n");
  1639. }
  1640. ret = sb_verify_commands(ictx, sctx, fp);
  1641. if (ret)
  1642. return ret;
  1643. sctx = sctx->sect;
  1644. }
  1645. /*
  1646. * FIXME IDEA:
  1647. * check if the first TAG command is at sctx->section_offset
  1648. */
  1649. return 0;
  1650. }
  1651. static int sb_verify_image_end(struct sb_image_ctx *ictx,
  1652. FILE *fp, off_t filesz)
  1653. {
  1654. uint8_t digest[32];
  1655. unsigned long size;
  1656. off_t pos;
  1657. int ret;
  1658. soprintf(ictx, "------------- Verifying image end -------------\n");
  1659. size = fread(digest, 1, sizeof(digest), fp);
  1660. if (size != sizeof(digest)) {
  1661. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1662. return -EINVAL;
  1663. }
  1664. pos = ftell(fp);
  1665. if (pos != filesz) {
  1666. fprintf(stderr, "ERR: Trailing data past the image!\n");
  1667. return -EINVAL;
  1668. }
  1669. /* Check the image digest. */
  1670. EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
  1671. /* Decrypt the image digest from the input image. */
  1672. sb_aes_reinit(ictx, 0);
  1673. sb_aes_crypt(ictx, digest, digest, sizeof(digest));
  1674. /* Check all of 20 bytes of the SHA1 hash. */
  1675. ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0;
  1676. if (ret)
  1677. soprintf(ictx, "[FAIL] Full-image checksum: BAD\n");
  1678. else
  1679. soprintf(ictx, "[PASS] Full-image checksum: OK\n");
  1680. return ret;
  1681. }
  1682. static int sb_build_tree_from_img(struct sb_image_ctx *ictx)
  1683. {
  1684. long filesize;
  1685. int ret;
  1686. FILE *fp;
  1687. if (!ictx->input_filename) {
  1688. fprintf(stderr, "ERR: Missing filename!\n");
  1689. return -EINVAL;
  1690. }
  1691. fp = fopen(ictx->input_filename, "r");
  1692. if (!fp)
  1693. goto err_open;
  1694. ret = fseek(fp, 0, SEEK_END);
  1695. if (ret < 0)
  1696. goto err_file;
  1697. filesize = ftell(fp);
  1698. if (filesize < 0)
  1699. goto err_file;
  1700. ret = fseek(fp, 0, SEEK_SET);
  1701. if (ret < 0)
  1702. goto err_file;
  1703. if (filesize < (signed)sizeof(ictx->payload)) {
  1704. fprintf(stderr, "ERR: File too short!\n");
  1705. goto err_file;
  1706. }
  1707. if (filesize & (SB_BLOCK_SIZE - 1)) {
  1708. fprintf(stderr, "ERR: The file is not aligned!\n");
  1709. goto err_file;
  1710. }
  1711. /* Load and verify image header */
  1712. ret = sb_verify_image_header(ictx, fp, filesize);
  1713. if (ret)
  1714. goto err_verify;
  1715. /* Load and verify sections and commands */
  1716. ret = sb_verify_sections_cmds(ictx, fp);
  1717. if (ret)
  1718. goto err_verify;
  1719. ret = sb_verify_image_end(ictx, fp, filesize);
  1720. if (ret)
  1721. goto err_verify;
  1722. ret = 0;
  1723. err_verify:
  1724. soprintf(ictx, "-------------------- Result -------------------\n");
  1725. soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED");
  1726. /* Stop the encryption session. */
  1727. sb_aes_deinit(&ictx->cipher_ctx);
  1728. fclose(fp);
  1729. return ret;
  1730. err_file:
  1731. fclose(fp);
  1732. err_open:
  1733. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1734. ictx->input_filename);
  1735. return -EINVAL;
  1736. }
  1737. static void sb_free_image(struct sb_image_ctx *ictx)
  1738. {
  1739. struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
  1740. struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head;
  1741. struct sb_cmd_ctx *cctx, *c_head;
  1742. while (sctx) {
  1743. s_head = sctx;
  1744. c_head = sctx->cmd_head;
  1745. while (c_head) {
  1746. cctx = c_head;
  1747. c_head = c_head->cmd;
  1748. if (cctx->data)
  1749. free(cctx->data);
  1750. free(cctx);
  1751. }
  1752. sctx = sctx->sect;
  1753. free(s_head);
  1754. }
  1755. while (dctx) {
  1756. d_head = dctx;
  1757. dctx = dctx->dcd;
  1758. free(d_head->payload);
  1759. free(d_head);
  1760. }
  1761. }
  1762. /*
  1763. * MXSSB-MKIMAGE glue code.
  1764. */
  1765. static int mxsimage_check_image_types(uint8_t type)
  1766. {
  1767. if (type == IH_TYPE_MXSIMAGE)
  1768. return EXIT_SUCCESS;
  1769. else
  1770. return EXIT_FAILURE;
  1771. }
  1772. static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd,
  1773. struct image_tool_params *params)
  1774. {
  1775. }
  1776. int mxsimage_check_params(struct image_tool_params *params)
  1777. {
  1778. if (!params)
  1779. return -1;
  1780. if (!strlen(params->imagename)) {
  1781. fprintf(stderr,
  1782. "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n",
  1783. params->cmdname);
  1784. return -1;
  1785. }
  1786. /*
  1787. * Check parameters:
  1788. * XIP is not allowed and verify that incompatible
  1789. * parameters are not sent at the same time
  1790. * For example, if list is required a data image must not be provided
  1791. */
  1792. return (params->dflag && (params->fflag || params->lflag)) ||
  1793. (params->fflag && (params->dflag || params->lflag)) ||
  1794. (params->lflag && (params->dflag || params->fflag)) ||
  1795. (params->xflag) || !(strlen(params->imagename));
  1796. }
  1797. static int mxsimage_verify_print_header(char *file, int silent)
  1798. {
  1799. int ret;
  1800. struct sb_image_ctx ctx;
  1801. memset(&ctx, 0, sizeof(ctx));
  1802. ctx.input_filename = file;
  1803. ctx.silent_dump = silent;
  1804. ret = sb_build_tree_from_img(&ctx);
  1805. sb_free_image(&ctx);
  1806. return ret;
  1807. }
  1808. char *imagefile;
  1809. static int mxsimage_verify_header(unsigned char *ptr, int image_size,
  1810. struct image_tool_params *params)
  1811. {
  1812. struct sb_boot_image_header *hdr;
  1813. if (!ptr)
  1814. return -EINVAL;
  1815. hdr = (struct sb_boot_image_header *)ptr;
  1816. /*
  1817. * Check if the header contains the MXS image signatures,
  1818. * if so, do a full-image verification.
  1819. */
  1820. if (memcmp(hdr->signature1, "STMP", 4) ||
  1821. memcmp(hdr->signature2, "sgtl", 4))
  1822. return -EINVAL;
  1823. imagefile = params->imagefile;
  1824. return mxsimage_verify_print_header(params->imagefile, 1);
  1825. }
  1826. static void mxsimage_print_header(const void *hdr)
  1827. {
  1828. if (imagefile)
  1829. mxsimage_verify_print_header(imagefile, 0);
  1830. }
  1831. static int sb_build_image(struct sb_image_ctx *ictx,
  1832. struct image_type_params *tparams)
  1833. {
  1834. struct sb_boot_image_header *sb_header = &ictx->payload;
  1835. struct sb_section_ctx *sctx;
  1836. struct sb_cmd_ctx *cctx;
  1837. struct sb_command *ccmd;
  1838. struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
  1839. uint8_t *image, *iptr;
  1840. /* Calculate image size. */
  1841. uint32_t size = sizeof(*sb_header) +
  1842. ictx->sect_count * sizeof(struct sb_sections_header) +
  1843. sizeof(*sb_dict_key) + sizeof(ictx->digest);
  1844. sctx = ictx->sect_head;
  1845. while (sctx) {
  1846. size += sctx->size;
  1847. sctx = sctx->sect;
  1848. };
  1849. image = malloc(size);
  1850. if (!image)
  1851. return -ENOMEM;
  1852. iptr = image;
  1853. memcpy(iptr, sb_header, sizeof(*sb_header));
  1854. iptr += sizeof(*sb_header);
  1855. sctx = ictx->sect_head;
  1856. while (sctx) {
  1857. memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header));
  1858. iptr += sizeof(struct sb_sections_header);
  1859. sctx = sctx->sect;
  1860. };
  1861. memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key));
  1862. iptr += sizeof(*sb_dict_key);
  1863. sctx = ictx->sect_head;
  1864. while (sctx) {
  1865. cctx = sctx->cmd_head;
  1866. while (cctx) {
  1867. ccmd = &cctx->payload;
  1868. memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload));
  1869. iptr += sizeof(cctx->payload);
  1870. if (ccmd->header.tag == ROM_LOAD_CMD) {
  1871. memcpy(iptr, cctx->data, cctx->length);
  1872. iptr += cctx->length;
  1873. }
  1874. cctx = cctx->cmd;
  1875. }
  1876. sctx = sctx->sect;
  1877. };
  1878. memcpy(iptr, ictx->digest, sizeof(ictx->digest));
  1879. iptr += sizeof(ictx->digest);
  1880. /* Configure the mkimage */
  1881. tparams->hdr = image;
  1882. tparams->header_size = size;
  1883. return 0;
  1884. }
  1885. static int mxsimage_generate(struct image_tool_params *params,
  1886. struct image_type_params *tparams)
  1887. {
  1888. int ret;
  1889. struct sb_image_ctx ctx;
  1890. /* Do not copy the U-Boot image! */
  1891. params->skipcpy = 1;
  1892. memset(&ctx, 0, sizeof(ctx));
  1893. ctx.cfg_filename = params->imagename;
  1894. ctx.output_filename = params->imagefile;
  1895. ret = sb_build_tree_from_cfg(&ctx);
  1896. if (ret)
  1897. goto fail;
  1898. ret = sb_encrypt_image(&ctx);
  1899. if (!ret)
  1900. ret = sb_build_image(&ctx, tparams);
  1901. fail:
  1902. sb_free_image(&ctx);
  1903. return ret;
  1904. }
  1905. /*
  1906. * mxsimage parameters
  1907. */
  1908. static struct image_type_params mxsimage_params = {
  1909. .name = "Freescale MXS Boot Image support",
  1910. .header_size = 0,
  1911. .hdr = NULL,
  1912. .check_image_type = mxsimage_check_image_types,
  1913. .verify_header = mxsimage_verify_header,
  1914. .print_header = mxsimage_print_header,
  1915. .set_header = mxsimage_set_header,
  1916. .check_params = mxsimage_check_params,
  1917. .vrec_header = mxsimage_generate,
  1918. };
  1919. void init_mxs_image_type(void)
  1920. {
  1921. register_image_type(&mxsimage_params);
  1922. }
  1923. #else
  1924. void init_mxs_image_type(void)
  1925. {
  1926. }
  1927. #endif