mxsimage.c 50 KB

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